卡尔曼滤波

最新推荐文章于 2025-06-08 18:14:43 发布

less interests，more interest

最新推荐文章于 2025-06-08 18:14:43 发布

阅读量142

点赞数 2

文章标签：算法人工智能

本文链接：https://blog.youkuaiyun.com/qq_61568285/article/details/136463984

版权

//先验估计 Xk_minus=A*X_(k-1)+B*U_(k-1)
X_minus[0]=A[0]*X[0]+A[1]*X[1]+B[0]*Gyro_Z;
X_minus[1]=A[2]*X[0]+A[3]*X[1]+B[1]*Gyro_Z;

//先验误差协方差求解 Pk_minus=A*P_(k-1)*A'+Q
P_minus[0]=(A[0]*P[0]+A[1]*P[2])*A[0]+(A[0]*P[1]+A[1]*P[3])*A[1]+Q[0];
P_minus[1]=(A[0]*P[0]+A[1]*P[2])*A[2]+(A[0]*P[1]+A[1]*P[3])*A[3]+Q[1];
P_minus[2]=(A[2]*P[0]+A[3]*P[2])*A[0]+(A[2]*P[1]+A[3]*P[3])*A[1]+Q[2];
P_minus[3]=(A[2]*P[0]+A[3]*P[2])*A[2]+(A[2]*P[1]+A[3]*P[3])*A[3]+Q[3];

//卡尔曼增益计算  Kk=Pk_minus*H'/(H*Pk_minus*H'+R)
double m=(H[0]*P_minus[0]+H[1]*P_minus[2])*H[0]+(H[0]*P_minus[1]+H[1]*P_minus[3])*H[1]+R;
K_k[0]=(P_minus[0]*H[0]+P_minus[1]*H[1])/m;
K_k[1]=(P_minus[2]*H[0]+P_minus[3]*H[1])/m;

//后验估计 X_k=Xk_minus+K_k*(Zk-H*Xk_minus)
X[0]=X_minus[0]+K_k[0]*(Z_k-(H[0]*X_minus[0]+H[1]*X_minus[1]));
X[1]=X_minus[1]+K_k[1]*(Z_k-(H[0]*X_minus[0]+H[1]*X_minus[1]));

//更新误差协方差 P_k=(I-K_k*H)*Pk_minus
P[0]=(1-K_k[0]*H[0])*P_minus[0]+(-K_k[0]*H[1])*P_minus[2];
P[1]=(1-K_k[0]*H[0])*P_minus[1]+(-K_k[0]*H[1])*P_minus[3];
P[2]=-K_k[1]*H[0]*P_minus[0]+(1-K_k[1]*H[1])*P_minus[2];
P[2]=-K_k[1]*H[0]*P_minus[1]+(1-K_k[1]*H[1])*P_minus[3];