Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Gyro Kalman filter from EmuFlight (#5519)
* Direct copy from EmuFlight, compiles, does not wrk * Fix Kalman computation * Catchup on Emu implementation * Make Q, W and Sharpness configurable * Settings for Kalman Q, W and Sharpness * Make it possible to enable/disable Kalman filter * Change scaling to make initial values simpler * Change Kalman constrains * Compute real variance * Drop unused function parameter * Improve EKF processing for gyro
- Loading branch information
Showing
9 changed files
with
230 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -72,5 +72,6 @@ typedef enum { | |
DEBUG_DYNAMIC_FILTER_FREQUENCY, | ||
DEBUG_IRLOCK, | ||
DEBUG_CD, | ||
DEBUG_KALMAN, | ||
DEBUG_COUNT | ||
} debugType_e; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,126 @@ | ||
/* | ||
* This file is part of Cleanflight and Betaflight. | ||
* | ||
* Cleanflight and Betaflight are free software. You can redistribute | ||
* this software and/or modify this software under the terms of the | ||
* GNU General Public License as published by the Free Software | ||
* Foundation, either version 3 of the License, or (at your option) | ||
* any later version. | ||
* | ||
* Cleanflight and Betaflight are distributed in the hope that they | ||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied | ||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. | ||
* See the GNU General Public License for more details. | ||
* | ||
* You should have received a copy of the GNU General Public License | ||
* along with this software. | ||
* | ||
* If not, see <http://www.gnu.org/licenses/>. | ||
*/ | ||
#include "platform.h" | ||
#ifdef USE_GYRO_KALMAN | ||
|
||
FILE_COMPILE_FOR_SPEED | ||
|
||
#include <string.h> | ||
#include "arm_math.h" | ||
|
||
#include "kalman.h" | ||
#include "build/debug.h" | ||
|
||
kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT]; | ||
float setPoint[XYZ_AXIS_COUNT]; | ||
|
||
static void gyroKalmanInitAxis(kalman_t *filter) | ||
{ | ||
memset(filter, 0, sizeof(kalman_t)); | ||
filter->q = gyroConfig()->kalman_q * 0.03f; //add multiplier to make tuning easier | ||
filter->r = 88.0f; //seeding R at 88.0f | ||
filter->p = 30.0f; //seeding P at 30.0f | ||
filter->e = 1.0f; | ||
filter->s = gyroConfig()->kalman_sharpness / 10.0f; | ||
filter->w = gyroConfig()->kalman_w * 8; | ||
filter->inverseN = 1.0f / (float)(filter->w); | ||
} | ||
|
||
void gyroKalmanSetSetpoint(uint8_t axis, float rate) | ||
{ | ||
setPoint[axis] = rate; | ||
} | ||
|
||
void gyroKalmanInitialize(void) | ||
{ | ||
gyroKalmanInitAxis(&kalmanFilterStateRate[X]); | ||
gyroKalmanInitAxis(&kalmanFilterStateRate[Y]); | ||
gyroKalmanInitAxis(&kalmanFilterStateRate[Z]); | ||
} | ||
|
||
float kalman_process(kalman_t *kalmanState, float input, float target) | ||
{ | ||
float targetAbs = fabsf(target); | ||
//project the state ahead using acceleration | ||
kalmanState->x += (kalmanState->x - kalmanState->lastX); | ||
|
||
//figure out how much to boost or reduce our error in the estimate based on setpoint target. | ||
//this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint. | ||
//update last state | ||
kalmanState->lastX = kalmanState->x; | ||
|
||
if (kalmanState->lastX != 0.0f) | ||
{ | ||
// calculate the error and add multiply sharpness boost | ||
float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s; | ||
|
||
// give a boost to the setpoint, used to caluclate the kalman q, based on the error and setpoint/gyrodata | ||
errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f); | ||
|
||
kalmanState->e = fabsf(1.0f - (((targetAbs + 1.0f) * errorMultiplier) / fabsf(kalmanState->lastX))); | ||
} | ||
|
||
//prediction update | ||
kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e); | ||
|
||
//measurement update | ||
kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r); | ||
kalmanState->x += kalmanState->k * (input - kalmanState->x); | ||
kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p; | ||
return kalmanState->x; | ||
} | ||
|
||
static void updateAxisVariance(kalman_t *kalmanState, float rate) | ||
{ | ||
kalmanState->axisWindow[kalmanState->windex] = rate; | ||
|
||
kalmanState->axisSumMean += kalmanState->axisWindow[kalmanState->windex]; | ||
float varianceElement = kalmanState->axisWindow[kalmanState->windex] - kalmanState->axisMean; | ||
varianceElement = varianceElement * varianceElement; | ||
kalmanState->axisSumVar += varianceElement; | ||
kalmanState->varianceWindow[kalmanState->windex] = varianceElement; | ||
|
||
kalmanState->windex++; | ||
if (kalmanState->windex >= kalmanState->w) { | ||
kalmanState->windex = 0; | ||
} | ||
|
||
kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex]; | ||
kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex]; | ||
|
||
//New mean | ||
kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN; | ||
kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN; | ||
|
||
float squirt; | ||
arm_sqrt_f32(kalmanState->axisVar, &squirt); | ||
kalmanState->r = squirt * VARIANCE_SCALE; | ||
} | ||
|
||
float gyroKalmanUpdate(uint8_t axis, float input) | ||
{ | ||
updateAxisVariance(&kalmanFilterStateRate[axis], input); | ||
|
||
DEBUG_SET(DEBUG_KALMAN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain | ||
|
||
return kalman_process(&kalmanFilterStateRate[axis], input, setPoint[axis]); | ||
} | ||
|
||
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,54 @@ | ||
/* | ||
* This file is part of Cleanflight and Betaflight. | ||
* | ||
* Cleanflight and Betaflight are free software. You can redistribute | ||
* this software and/or modify this software under the terms of the | ||
* GNU General Public License as published by the Free Software | ||
* Foundation, either version 3 of the License, or (at your option) | ||
* any later version. | ||
* | ||
* Cleanflight and Betaflight are distributed in the hope that they | ||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied | ||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. | ||
* See the GNU General Public License for more details. | ||
* | ||
* You should have received a copy of the GNU General Public License | ||
* along with this software. | ||
* | ||
* If not, see <http://www.gnu.org/licenses/>. | ||
*/ | ||
|
||
#pragma once | ||
|
||
#include "sensors/gyro.h" | ||
#include "common/filter.h" | ||
|
||
#define MAX_KALMAN_WINDOW_SIZE 512 | ||
|
||
#define VARIANCE_SCALE 0.67f | ||
|
||
typedef struct kalman | ||
{ | ||
float q; //process noise covariance | ||
float r; //measurement noise covariance | ||
float p; //estimation error covariance matrix | ||
float k; //kalman gain | ||
float x; //state | ||
float lastX; //previous state | ||
float e; | ||
float s; | ||
|
||
float axisVar; | ||
uint16_t windex; | ||
float axisWindow[MAX_KALMAN_WINDOW_SIZE]; | ||
float varianceWindow[MAX_KALMAN_WINDOW_SIZE]; | ||
float axisSumMean; | ||
float axisMean; | ||
float axisSumVar; | ||
float inverseN; | ||
uint16_t w; | ||
} kalman_t; | ||
|
||
void gyroKalmanInitialize(void); | ||
float gyroKalmanUpdate(uint8_t axis, float input); | ||
void gyroKalmanSetSetpoint(uint8_t axis, float rate); |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
706da4a
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@DzikuVx , in looking for the D-term cap limit for this iNAV tune I'm doing, I saw this Commit.
check out the site: http://cristal.univ-lille.fr/~casiez/1euro/InteractiveDemo/
and then view-source:http://cristal.univ-lille.fr/~casiez/1euro/InteractiveDemo/filters.js
I sent to Kevin. If you have some that knows Java well (or yourself), maybe they could translate to 'C' so that the key filter equation for the "Kalman" doesn't terminate in a PT1 lowpass filter as it does with the current implementation.
See tiny.cc/uavtech for the "Kalman Calcs.xlsb" which models against a static PT1. Ultimately, the current implementation is a dynamic PT1 that modulates the cutoff by the Co-variance of the past gyro signal. Also weird in the current implementations it moves the cutoff DOWN as the sticks go to their extents and not the opposite. If you look at noise in blackbox, it goes down as you move to stick extents and not up. This is do mostly due throttle being low at stick extents and also frame stresses I'm sure to contribute to the noise reduction. So the current implementation seems backward. Folks at Emu have dismissed as "whelp, it is still amazing" but that is NOT science. If you don't understand and have a logical train of thought of why something is better to at least start with, that means it is not better or you have not done enough study yet.
Another thing that doesn't help is the Co-variance of past samples doesn't help for prop wash because it is too late; the keyword is PAST samples.
I hope this helps to pass on analysis done to the date of which raises questions in regard to the filter actions and efficacy. The first key thing to me is seeing if something big was missed in the "Kalman" implementation to terminate in a lowpass filter vs. another technique. I have not devoted the time to decrypt the Java code nor to look at guides at other sources because my hunch is no matter how you cut it, it's a lowpass filter and by design, they don't cut enough, fast enough - without adding huge delay (3 or 4th order questions) to outperform notches at killing motor peaks.
The fundamental of a true Kalman is taking multiple data sources - from multiple sensors - to make a true prediction (hopefully with a system model) and use that to filter data from a gyro or something. The "Kalman" from all the hype never did that. Even the "prediction" in the current implementation is nothing more than using the slope of the last two gyro samples to project the next sample position and use that in the PT1 filter equation instead of the last sample. When you break that down, that ADDS noise to the output and does NOT reduce (see excel sheet and FKF modeling). Secondary, it is hardly a prediction using the Accelerometer as some will say.