卡尔曼滤波为什么反应很慢
最近在调平衡车的程序，看了很多关于卡尔曼滤波的文章，代码很简单，但是要想理解其中的精髓真的很难！现在调试的时候出现了如下问题
当预值变化很快的时候，滤波后的数据，反应迟缓，并很长时间才能跟上来！！
有谁以前调过卡尔曼的，请指教！！！这是什么问题
运算量太大.
可以考虑减少阶层或把系数因定.不过结果会受些影响. Firman 发表于 2013-12-31 16:23
运算量太大.
可以考虑减少阶层或把系数因定.不过结果会受些影响.
现在运算时间为10ms ，可能因为时间太长！！
是不是把 dt改小啊？？？？？？
#define Q_angle 0.001
#define Q_gyro 0.003
#define R_angle 0.5
#define dt 0.01 // dt的取值为kalman滤波器采样时间,此处0.01为10ms;
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;//输出值(后验估计)的微分=角速度
} 谭春林 发表于 2013-12-31 16:58
现在运算时间为10ms ，可能因为时间太长！！
是不是把 dt改小啊？？？？？？
这种写法是把Angle和Q_bias作为状态量么、angle_m和gyro_m作为观测量么？
为什么状态预测方程里面会有观测量gyro_m呢？ K.O.Carnivist 发表于 2014-1-1 10:31
这种写法是把Angle和Q_bias作为状态量么、angle_m和gyro_m作为观测量么？
为什么状态预测方程里面会有观 ...
状态量是要根据观察量来计算的啊，反正我也不是很懂这个程序，网上用的都是这个程序！！ 一方面是运算量太大，另一方面是自适应滤波是基于前后相关的运算。 rmdyj 发表于 2014-1-1 11:03
一方面是运算量太大，另一方面是自适应滤波是基于前后相关的运算。
那如何解决这个问题呢！！！ 谭春林 发表于 2014-1-1 10:40
状态量是要根据观察量来计算的啊，反正我也不是很懂这个程序，网上用的都是这个程序！！ ...
状态预测是状态转移方程，观测量应该只在状态更新里面出现。
我感觉一个比较直观的做法是把角度、角速度和那个Q_bias一起作为的状态量，这样在预测方程里计算角度时出现的是前一次状态更新得到的角速度，陀螺仪得到的角速度作为观测量出现在更新过程的观测余量里。
才反应过来3楼程序的意思好像是，陀螺仪角速度测量值是强制输入量u，这样状态预测就会带有陀螺仪测量值，观测量只有angle_m。
论坛图片好像暂时挂了，等回复了再看下楼主位…… 这个跟Q有关，，Q越小代表系统测量误差小，即更加相信测量值，这样变化就会慢。。把Q改大试试 knight200300 发表于 2014-1-1 14:03
这个跟Q有关，，Q越小代表系统测量误差小，即更加相信测量值，这样变化就会慢。。把Q改大试试 ...
可是我看别人用的滤波程序参数就是这个，不知道他们为什么可以用！！ 谭春林 发表于 2014-1-1 15:47
可是我看别人用的滤波程序参数就是这个，不知道他们为什么可以用！！
这个Q跟你的系统有关啊，，这里面包括了传感器的噪声什么的，肯定每个都有差异的 用什么单片机啊？
mruio 发表于 2014-1-2 10:44
用什么单片机啊？
STM32啊 谭春林 发表于 2014-1-2 11:48
STM32啊
数据不经过滤波，显示正常不？
mruio 发表于 2014-1-3 20:46
数据不经过滤波，显示正常不？
很正常啊 现在不知道初始值的设定有什么技巧 Q和R的设定至今还是一个难题，不过有经验方法，我还在寻找，一般的做法就是设成一个很小的数（矩阵） dt越小，变化越慢，不信你试试改为0.05~0.5！！！
页:
[1]