src/main/flight/kalman.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20 #include "platform.h"
  21 #ifdef USE_GYRO_KALMAN
  22
  23 FILE_COMPILE_FOR_SPEED
  24
  25 #include <string.h>
  26 #include "arm_math.h"
  27
  28 #include "kalman.h"
  29 #include "build/debug.h"
  30
  31 kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
  32 float setPoint[XYZ_AXIS_COUNT];
  33
  34 static void gyroKalmanInitAxis(kalman_t *filter)
  35 {
  36     memset(filter, 0, sizeof(kalman_t));
  37     filter->q = gyroConfig()->kalman_q * 0.03f; //add multiplier to make tuning easier
  38     filter->r = 88.0f;      //seeding R at 88.0f
  39     filter->p = 30.0f;      //seeding P at 30.0f
  40     filter->e = 1.0f;
  41     filter->s = gyroConfig()->kalman_sharpness / 10.0f;
  42     filter->w = gyroConfig()->kalman_w * 8;
  43     filter->inverseN = 1.0f / (float)(filter->w);
  44 }
  45
  46 void gyroKalmanSetSetpoint(uint8_t axis, float rate)
  47 {
  48     setPoint[axis] = rate;
  49 }
  50
  51 void gyroKalmanInitialize(void)
  52 {
  53     gyroKalmanInitAxis(&kalmanFilterStateRate[X]);
  54     gyroKalmanInitAxis(&kalmanFilterStateRate[Y]);
  55     gyroKalmanInitAxis(&kalmanFilterStateRate[Z]);
  56 }
  57
  58 float kalman_process(kalman_t *kalmanState, float input, float target)
  59 {
  60     float targetAbs = fabsf(target);
  61     //project the state ahead using acceleration
  62     kalmanState->x += (kalmanState->x - kalmanState->lastX);
  63
  64     //figure out how much to boost or reduce our error in the estimate based on setpoint target.
  65     //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint.
  66     //update last state
  67     kalmanState->lastX = kalmanState->x;
  68
  69     if (kalmanState->lastX != 0.0f)
  70     {
  71         // calculate the error and add multiply sharpness boost
  72         float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
  73
  74         // give a boost to the setpoint, used to caluclate the kalman q, based on the error and setpoint/gyrodata
  75         errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
  76
  77         kalmanState->e = fabsf(1.0f - (((targetAbs + 1.0f) * errorMultiplier) / fabsf(kalmanState->lastX)));
  78     }
  79
  80     //prediction update
  81     kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e);
  82
  83     //measurement update
  84     kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r);
  85     kalmanState->x += kalmanState->k * (input - kalmanState->x);
  86     kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p;
  87     return kalmanState->x;
  88 }
  89
  90 static void updateAxisVariance(kalman_t *kalmanState, float rate)
  91 {
  92     kalmanState->axisWindow[kalmanState->windex] = rate;
  93
  94     kalmanState->axisSumMean += kalmanState->axisWindow[kalmanState->windex];
  95     float varianceElement = kalmanState->axisWindow[kalmanState->windex] - kalmanState->axisMean;
  96     varianceElement = varianceElement * varianceElement;
  97     kalmanState->axisSumVar += varianceElement;
  98     kalmanState->varianceWindow[kalmanState->windex] = varianceElement;
  99
 100     kalmanState->windex++;
 101     if (kalmanState->windex >= kalmanState->w) {
 102         kalmanState->windex = 0;
 103     }
 104
 105     kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex];
 106     kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex];
 107
 108     //New mean
 109     kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN;
 110     kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN;
 111
 112     float squirt;
 113     arm_sqrt_f32(kalmanState->axisVar, &squirt);
 114     kalmanState->r = squirt * VARIANCE_SCALE;
 115 }
 116
 117 float gyroKalmanUpdate(uint8_t axis, float input)
 118 {
 119     updateAxisVariance(&kalmanFilterStateRate[axis], input);
 120
 121     DEBUG_SET(DEBUG_KALMAN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain
 122
 123     return kalman_process(&kalmanFilterStateRate[axis], input, setPoint[axis]);
 124 }
 125
 126 #endif