卡尔曼滤波
发布网友
发布时间：2022-04-24 23:57
共1个回答
热心网友
时间：2023-10-15 17:14
这里的Pdot是一个中间变量，你只看几个步骤是不可能会懂的，最好要全部一起看，下面是纤细步骤：
这里用到的kalman主要分为5个步骤：
(1) X=A*X+B*angular_speed_m;
(2) P=A*P*A'+Q;
(3) X=X-KG(Z-H*X);
(4) K=P*H'(H*P*H'+R);
(5) (I-KG*H)*P;
对矩阵P的更新只有 P=A*P*A'+Q; P,A,Q都是矩阵
A=1 -TS
0 1
Q=q_acce 0
0 q_gyro
单片机目前肯定是不能计算矩阵啦，说以就自己算矩阵乘法，加法(线性代数)
P[0][0]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[0][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+KAL_Q[0][0];/////////////////////
P[0][1]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[1][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[1][1]+KAL_Q[0][1];////////////////////
P[1][0]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[0][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[0][1]+KAL_Q[1][0];////////////////////
P[1][1]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[1][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[1][1]+KAL_Q[1][1];////////////////////
然后把等于1或0的式子直接去掉
P[0][0]=(P[0][0]+KAL_A[0][1]*P[1][0])+(P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+q_acce;// a
P[0][1]=P[0][1]+KAL_A[0][1]*P[1][1];// b
P[1][0]=P[1][0]+P[1][1]*KAL_A[0][1];// c
P[1][1]=P[1][1]+q_gyro;// d
下面是你写的
P[0][0] += Pdot[0] * dt;
P[1][1] += Pdot[3] * dt;
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[3] = Q_gyro;
简化，约分后
P[0][0] +=（Q_angle - P[0][1] - P[1][0]）* dt； >>对应上面的a
P[1][1] +=Q_gyro * dt; >>对应上面的d
当然我自建的kalman和你看的那个版本还是有点差异，不过我的这个版本是可以用的，你看的哪个版本也可以用，效果都差不多。
我的回答就是这样，记得点赞哦！！！！追问赞个，我也觉得你的是对的。那个版本Pdot感觉乱乘了采样时间dt
追答是的，在这个式子里面
P[0][0] +=（Q_angle - P[0][1] - P[1][0]）* dt；
我感觉Q_angle无论怎么弄都应该放在括号的外面，因为方程(2) P=A*P*A'+Q;这里的Q矩阵是加，那么最终在P的结果里Q矩阵的元素也应该只会以加的形式出现。
我想可能是他建立的状态方程和我的不一样
