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 #include <string.h>
  24 #if !defined(SITL_BUILD)
  25 #include "arm_math.h"
  26 #else
  27 #include <math.h>
  28 #endif
  29
  30 #include "kalman.h"
  31 #include "build/debug.h"
  32
  33 kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
  34
  35 static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q)
  36 {
  37     memset(filter, 0, sizeof(kalman_t));
  38     filter->q = q * 0.03f; //add multiplier to make tuning easier
  39     filter->r = 88.0f;      //seeding R at 88.0f
  40     filter->p = 30.0f;      //seeding P at 30.0f
  41     filter->e = 1.0f;
  42     filter->w = MAX_KALMAN_WINDOW_SIZE;
  43     filter->inverseN = 1.0f / (float)(filter->w);
  44 }
  45
  46 void gyroKalmanInitialize(uint16_t q)
  47 {
  48     gyroKalmanInitAxis(&kalmanFilterStateRate[X], q);
  49     gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q);
  50     gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q);
  51 }
  52
  53 float kalman_process(kalman_t *kalmanState, float input)
  54 {
  55     //project the state ahead using acceleration
  56     kalmanState->x += (kalmanState->x - kalmanState->lastX);
  57
  58     //update last state
  59     kalmanState->lastX = kalmanState->x;
  60
  61     if (kalmanState->lastX != 0.0f)
  62     {
  63         kalmanState->e = fabsf(1.0f - (kalmanState->setpoint / kalmanState->lastX));
  64     }
  65
  66     //prediction update
  67     kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e);
  68
  69     //measurement update
  70     kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r);
  71     kalmanState->x += kalmanState->k * (input - kalmanState->x);
  72     kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p;
  73     return kalmanState->x;
  74 }
  75
  76 static void updateAxisVariance(kalman_t *kalmanState, float rate)
  77 {
  78     kalmanState->axisWindow[kalmanState->windex] = rate;
  79
  80     kalmanState->axisSumMean += kalmanState->axisWindow[kalmanState->windex];
  81     float varianceElement = kalmanState->axisWindow[kalmanState->windex] - kalmanState->axisMean;
  82     varianceElement = varianceElement * varianceElement;
  83     kalmanState->axisSumVar += varianceElement;
  84     kalmanState->varianceWindow[kalmanState->windex] = varianceElement;
  85
  86     kalmanState->windex++;
  87     if (kalmanState->windex > kalmanState->w) {
  88         kalmanState->windex = 0;
  89     }
  90
  91     kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex];
  92     kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex];
  93
  94     //New mean
  95     kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN;
  96     kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN;
  97
  98 #if !defined(SITL_BUILD)
  99     float squirt;
 100     arm_sqrt_f32(kalmanState->axisVar, &squirt);
 101 #else
 102     float squirt = sqrtf(kalmanState->axisVar);
 103 #endif
 104
 105     kalmanState->r = squirt * VARIANCE_SCALE;
 106 }
 107
 108 float NOINLINE gyroKalmanUpdate(uint8_t axis, float input)
 109 {
 110     updateAxisVariance(&kalmanFilterStateRate[axis], input);
 111     return kalman_process(&kalmanFilterStateRate[axis], input);
 112 }
 113
 114 void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint) {
 115     kalmanFilterStateRate[axis].setpoint = setpoint;
 116 }
 117
 118 #endif