飞控中使用卡尔曼滤波的参数取值方法
本帖最后由 leicheng 于 2014-1-28 11:54 编辑卡尔曼滤波的一些参数取值方法:
(1) 初始的状态变量X0影响最小，可以直接取值为第一个测量值，在滤波可以收敛的情况下会很快收敛;
(2) 只要不为零，初始协方差矩阵P0的取值对滤波效果影响很小，都能很快收敛，可以任意取一个不为零的矩阵;
(3) 当状态转换过程为已确定时，Q的取值越小越好，可以使用一个非常小但不为零的矩阵;
(4) 测量噪声协方差R取值取值越小收敛越快，但滤波效果不一定好，可以在滤波前先测定噪声协方差，然后用于后续的滤波。 测量噪声要用ARMA定量分析 楼主你好，我刚开始学习使用卡尔曼滤波，网上找了个代码，在单片机中跑下来收敛很慢，做了个用随机数的模拟程序，同样收敛慢(一开始还可以，跳跃下就收敛慢了)，请教下卡尔曼那几个参数要怎么确定（按道理讲别人调好的，同样是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_01
//**********************************
static float P = {{ 1, 0 }, { 0, 1 }};
static float Pdot = {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=Q_angle - P - P; // Pk-先验估计误差协方差的微分
Pdot= - P;
Pdot= - P;
Pdot=Q_gyro;
P += Pdot * dt;// Pk-先验估计误差协方差微分的积分
P += Pdot * dt;// 先验估计误差协方差
P += Pdot * dt;
P += Pdot * dt;
Angle_err = angle_m - Angle;//zk-先验估计
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
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;
P -= K_0 * t_0;//后验估计误差协方差
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= 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 画个图看到一开始收敛还较快，跳跃后很难收敛，这几个参数基本不懂，就想先把程序正常跑起来后面再仔细学习，不会发图，能帮忙看看吗，非常感谢。
没加跳跃的时候，1500个随机点，效果： 因为现实中角度会变化，所以加了几个跳跃，收敛就不行了：
页:
[1]