我正在做一个项目，我想通过使用安装在鞋子上的 IMU 来接收位置数据。所以我发现我必须使用卡尔曼滤波器来获取有效的位置数据。
这就是我开始挣扎的地方。
大多数示例使用 6 DoF-IMU 使用卡尔曼滤波器计算位置。但我尝试使用 Invensense 的 9-DoF IMU ICM20948。它使用 Mag、Acc 和 Gyro 计算输出四元数。然后我用原始的 XYZ 加速度和给定的四元数计算北、东、下方向的加速度。
所以我对卡尔曼滤波器的输入只有北、东和下方向的加速度。我如何设置卡尔曼滤波器矩阵才能在这 3 个方向上获得有效的位置数据？
我根据我在网上找到的一个例子自己尝试了它，但它似乎不起作用，而且位置数据也不可信：
x = [ pos_n , pos_e , pos_d , vel_n , vel_e ,vel_d ]
A = [1,0,0,T,0,0 ;
0,1,0,0,T,0 ;
0,0,1,0,0,T ;
0,0,0,1,0,0 ;
0,0,0,0,1,0 ;
0,0,0,0,0,1 ; ]
Bu = [ an*(T * T/2) ; ae * (T * T/2) ; ad * (T * T/2) ; an * T ; ae * T ; ad * T ]
H = [ 0,0,0,1,0,0 ; 0,0,0,0,1,0 ; 0,0,0,0,0,1 ]
z = [ -vel_n, -vel_e , -vel_d ]
R = [sigma_vel^2 , 0 , 0 ; 0 , sigma_vel^2 , 0 ; 0, 0 , sigma_vel^2]
P = 6x6 unit matrix
Q = 6x6 matrix with sigma_acceleration^2 on the main diagonal.
我实现了以下测量算法：
