因为要平滑IMU的加速度计输出的数据,包括x,y和z轴的加速度,所以这里我只用了三维。按照自己对卡尔曼滤波的理解自己写了个c++工程,如果有错误请指出。
项目源码:GitHub - JackJu-HIT/KalmanFilter: 基于自己之前搭好的ROS机器人平台,自己写了卡尔曼滤波算法对IMU的加速度xyz进行滤波处理。
下面代码就是核心:预测+测量更新。
Eigen::MatrixXd kalman_filter::predictAndupdate(Eigen::MatrixXd x,Eigen::MatrixXd z)
{
if(!isinitized)
{
P=Eigen::MatrixXd(3,3);
P<<1,0,0,
0,1,0,
0,0,1;
isinitized=true;
}
x=A*x;
P=A*P*(A.transpose())+Q;
Eigen::MatrixXd K=P*(H.transpose())*((H*P*(H.transpose())+R).inverse());
x=x+