卡尔曼真的很难懂啊 一起分析分析（网上最长看到得代码）
先贴代码吧。static float angle, angle_dot; //外部需要引用的变量
//Kalman滤波
//-------------------------------------------------------
//-------------------------------------------------------
staticfloat Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.5; //注意：dt的取值为kalman滤波器采样时间;
static float Pk = { {1, 0 }, {0, 1 }};
static float Pdot ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //角度角速度
{
angle = angle + (gyro_m-q_bias) * dt; //角度 = 角度+（角速度测量值-偏移）*时间dt
// X(k|k-1) = X(k-1|k-1) + U(k)(1) // X(k|k-1)= X(k-1|k-1)+ U(k)(1)
Pdot=Q_angle - Pk - Pk;
Pdot=- Pk;
Pdot=- Pk;
Pdot=Q_gyro;
Pk=Pk + Pdot * dt;
//P(k|k-1)
Pk=Pk + Pdot * dt;
Pk=Pk + Pdot * dt;
Pk=Pk + Pdot * dt;
angle_err = angle_m - angle;// Z(k)-H X(k|k-1)
//Z(k) - X(k|k-1)
PCt_0 = C_0 * Pk; //P(k|k-1)*H’
PCt_1 = C_0 * Pk;
E = R_angle +C_0 * PCt_0;
// R + (H P(k|k-1) H’)
K_0 = PCt_0 /E;
//Kg(k) = P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * Pk;
Pk =Pk - K_0 * t_0;
Pk =Pk - K_0 * t_1;
Pk =Pk - K_1 * t_0;
Pk =Pk - K_1 * t_1;
angle =angle + K_0 * angle_err;//X(k|k)= X(k|k-1) + Kg(k)(Z(k)-H X(k|k-1)) ……… (3)
q_bias =q_bias + K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}
第一句最好懂，就是卡尔曼的第一条公式，增加了q_bias 修正值。（这个值如何进行循环修正就不知道了）
Pdot数组和Pk数组就不太明白了，PK应该是协方差的意思吧。还有协方差到底是如何进行计算的也不太清楚。网上的资料太深了。
angle_err 好理解 公式三的部分值，。。。。。。。
我把自己觉得代码对于的公式都标出来了。当然也不一定对应了。。
其他的就没头绪了 ，网上资料也不好找，协方差和装置矩阵的计算也看不懂。。。
所以希望大家一起分析啦。就当学习咯。。。网上大牛的文章又看不懂，而且和代码对应不起来啊 帮顶。不懂这些。
ahuang227 发表于 2012-9-3 16:18 static/image/common/back.gif
帮顶。不懂这些。
谢谢支持 我也想分析，但好像先做模型吧 robotkid 发表于 2012-9-4 17:04 static/image/common/back.gif
我也想分析，但好像先做模型吧
这个不是网上很常见的加速度 、角速度 通过卡尔曼计算实际角度的代码么？
要什么模型啊。
本帖最后由 pww999 于 2012-9-4 17:45 编辑
学习了 顶个 帮顶正在学习 用互补滤波也可以啊 这个简单
页:
[1]