飞控中使用卡尔曼滤波的参数取值方法

leicheng

卡尔曼滤波的一些参数取值方法:
(1) 初始的状态变量X0影响最小，可以直接取值为第一个测量值，在滤波可以收敛的情况下会很快收敛;
(2) 只要不为零，初始协方差矩阵P0的取值对滤波效果影响很小，都能很快收敛，可以任意取一个不为零的矩阵;
(3) 当状态转换过程为已确定时，Q的取值越小越好，可以使用一个非常小但不为零的矩阵;
(4) 测量噪声协方差R取值取值越小收敛越快，但滤波效果不一定好，可以在滤波前先测定噪声协方差，然后用于后续的滤波。

westloveohyeah

测量噪声要用ARMA定量分析

bruce_gong

楼主你好，我刚开始学习使用卡尔曼滤波，网上找了个代码，在单片机中跑下来收敛很慢，做了个用随机数的模拟程序，同样收敛慢(一开始还可以，跳跃下就收敛慢了)，请教下卡尔曼那几个参数要怎么确定（按道理讲别人调好的，同样是MPU6050，我应该可以直接使用不是吗）：
#include "stdio.h"
#include "stdlib.h"

//*********************************
#define Q_angle 0.001
#define Q_gyro 0.0
#define R_angle 0.5
#define dt   0.01      //dt的取值为kalman滤波器采样时间,此处为10ms;
#define C_0  1

//**********************************
static float P[2][2] = {{ 1, 0 }, { 0, 1 }};       
static float Pdot[4] = {0,0,0,0};

static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;

//////////////////////////////////////////////////////
static float Angle = 0.0, Angle_dot;          //倾角值和倾角加速度值

/*
**********************************************
**函数名  ：void Kalman_Filter(float angle_m,float gyro_m)
**函数功能：输出陀螺仪测量的倾角加速度
**返回参数：无
**传入参数：angle_m - 倾角测量值
**          GYRO_XOUT - 倾角加速度测量值
**********************************************
*/
void Kalman_Filter(float angle_m, float gyro_m)        //gyro_m:gyro_measure
{
        Angle += (gyro_m-Q_bias) * dt;  //先验估计
       
        Pdot[0]=Q_angle - P[0][1] - P[1][0]; // Pk-先验估计误差协方差的微分
        Pdot[1]= - P[1][1];
        Pdot[2]= - P[1][1];
        Pdot[3]=Q_gyro;
       
        P[0][0] += Pdot[0] * dt;  // Pk-先验估计误差协方差微分的积分
        P[0][1] += Pdot[1] * dt;  // 先验估计误差协方差
        P[1][0] += Pdot[2] * dt;
        P[1][1] += Pdot[3] * dt;
       
        Angle_err = angle_m - Angle;  //zk-先验估计
       
        PCt_0 = C_0 * P[0][0];
        PCt_1 = C_0 * P[1][0];
       
        E = R_angle + C_0 * PCt_0;
       
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
       
        t_0 = PCt_0;
        t_1 = C_0 * P[0][1];

        P[0][0] -= K_0 * t_0;  //后验估计误差协方差
        P[0][1] -= K_0 * t_1;
        P[1][0] -= K_1 * t_0;
        P[1][1] -= K_1 * t_1;       
       
        Angle        += K_0 * Angle_err; //后验估计
        Q_bias        += K_1 * Angle_err; //后验估计
        Angle_dot = gyro_m-Q_bias;  //输出值(后验估计)的微分=角速度
}

void main(void)
{
        int acc = 0;
        int jump = 1;
        for(int a = 0; a < 1500; a++)
        {
                //增加几个跳跃看看收敛情况
                if(a > 300)
                        jump = 50;
               
                if(a > 600)
                        jump = 100;
                       
                if(a > 900)
                        jump = 1;
               
                //随机数作为加速度技的模拟输入
                acc = jump + rand()%100;

                Kalman_Filter((float)acc, 0.0);
                printf("%d,%d\r\n", acc, (int)Angle);
        }
}

和陀螺仪相关的参数我去掉了，应为我是静态使用测量倾角的。
在linux下面执行：
gcc -g -o test2 test2.c  -std=c99; ./test2 > result.csv
用excel打开result.csv 画个图看到一开始收敛还较快，跳跃后很难收敛，这几个参数基本不懂，就想先把程序正常跑起来后面再仔细学习，不会发图，能帮忙看看吗，非常感谢。

bruce_gong

没加跳跃的时候，1500个随机点，效果：

bruce_gong

因为现实中角度会变化，所以加了几个跳跃，收敛就不行了：