mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 08:15:26 +03:00
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
This commit is contained in:
parent
839a877397
commit
706da4aef2
9 changed files with 230 additions and 2 deletions
|
@ -104,6 +104,7 @@ COMMON_SRC = \
|
|||
flight/gyroanalyse.c \
|
||||
flight/rpm_filter.c \
|
||||
flight/dynamic_gyro_notch.c \
|
||||
flight/kalman.c \
|
||||
io/beeper.c \
|
||||
io/esc_serialshot.c \
|
||||
io/servo_sbus.c \
|
||||
|
|
|
@ -72,5 +72,6 @@ typedef enum {
|
|||
DEBUG_DYNAMIC_FILTER_FREQUENCY,
|
||||
DEBUG_IRLOCK,
|
||||
DEBUG_CD,
|
||||
DEBUG_KALMAN,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
|
|
@ -84,7 +84,7 @@ tables:
|
|||
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
|
||||
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
|
||||
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
|
||||
"IRLOCK", "CD"]
|
||||
"IRLOCK", "CD", "KALMAN"]
|
||||
- name: async_mode
|
||||
values: ["NONE", "GYRO", "ALL"]
|
||||
- name: aux_operator
|
||||
|
@ -203,6 +203,25 @@ groups:
|
|||
condition: USE_DYNAMIC_FILTERS
|
||||
min: 30
|
||||
max: 1000
|
||||
- name: gyro_kalman_enabled
|
||||
condition: USE_GYRO_KALMAN
|
||||
field: kalmanEnabled
|
||||
type: bool
|
||||
- name: gyro_kalman_q
|
||||
field: kalman_q
|
||||
condition: USE_GYRO_KALMAN
|
||||
min: 1
|
||||
max: 16000
|
||||
- name: gyro_kalman_w
|
||||
field: kalman_w
|
||||
condition: USE_GYRO_KALMAN
|
||||
min: 1
|
||||
max: 40
|
||||
- name: gyro_kalman_sharpness
|
||||
field: kalman_sharpness
|
||||
condition: USE_GYRO_KALMAN
|
||||
min: 1
|
||||
max: 16000
|
||||
- name: gyro_to_use
|
||||
condition: USE_DUAL_GYRO
|
||||
min: 0
|
||||
|
|
126
src/main/flight/kalman.c
Executable file
126
src/main/flight/kalman.c
Executable file
|
@ -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
|
54
src/main/flight/kalman.h
Executable file
54
src/main/flight/kalman.h
Executable file
|
@ -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);
|
|
@ -44,6 +44,7 @@ FILE_COMPILE_FOR_SPEED
|
|||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
#include "flight/kalman.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
|
@ -966,6 +967,9 @@ void FAST_CODE pidController(float dT)
|
|||
|
||||
// Limit desired rate to something gyro can measure reliably
|
||||
pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
|
||||
#ifdef USE_GYRO_KALMAN
|
||||
gyroKalmanSetSetpoint(axis, pidState[axis].rateTarget);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK
|
||||
|
|
|
@ -72,6 +72,7 @@ FILE_COMPILE_FOR_SPEED
|
|||
#include "flight/gyroanalyse.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
#include "flight/dynamic_gyro_notch.h"
|
||||
#include "flight/kalman.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
|
@ -119,7 +120,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
|||
.dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
|
||||
.dynamicGyroNotchQ = 120,
|
||||
.dynamicGyroNotchMinHz = 150,
|
||||
.dynamicGyroNotchEnabled = 0
|
||||
.dynamicGyroNotchEnabled = 0,
|
||||
.kalman_q = 100,
|
||||
.kalman_w = 4,
|
||||
.kalman_sharpness = 100,
|
||||
.kalmanEnabled = 0,
|
||||
);
|
||||
|
||||
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
|
||||
|
@ -307,6 +312,11 @@ bool gyroInit(void)
|
|||
}
|
||||
|
||||
gyroInitFilters();
|
||||
#ifdef USE_GYRO_KALMAN
|
||||
if (gyroConfig()->kalmanEnabled) {
|
||||
gyroKalmanInitialize();
|
||||
}
|
||||
#endif
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
|
||||
gyroDataAnalyseStateInit(
|
||||
|
@ -450,6 +460,14 @@ void FAST_CODE NOINLINE gyroUpdate()
|
|||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_KALMAN
|
||||
if (gyroConfig()->kalmanEnabled) {
|
||||
gyro.gyroADCf[X] = gyroKalmanUpdate(X, gyro.gyroADCf[X]);
|
||||
gyro.gyroADCf[Y] = gyroKalmanUpdate(Y, gyro.gyroADCf[Y]);
|
||||
gyro.gyroADCf[Z] = gyroKalmanUpdate(Z, gyro.gyroADCf[Z]);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
if (dynamicGyroNotchState.enabled) {
|
||||
gyroDataAnalyse(&gyroAnalyseState);
|
||||
|
|
|
@ -74,6 +74,10 @@ typedef struct gyroConfig_s {
|
|||
uint16_t dynamicGyroNotchQ;
|
||||
uint16_t dynamicGyroNotchMinHz;
|
||||
uint8_t dynamicGyroNotchEnabled;
|
||||
uint16_t kalman_q;
|
||||
uint16_t kalman_w;
|
||||
uint16_t kalman_sharpness;
|
||||
uint8_t kalmanEnabled;
|
||||
} gyroConfig_t;
|
||||
|
||||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||
|
|
|
@ -72,6 +72,7 @@
|
|||
#define USE_PITOT_VIRTUAL
|
||||
|
||||
#define USE_DYNAMIC_FILTERS
|
||||
#define USE_GYRO_KALMAN
|
||||
#define USE_EXTENDED_CMS_MENUS
|
||||
#define USE_UAV_INTERCONNECT
|
||||
#define USE_RX_UIB
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue