0

我正在做一个项目，我想通过使用安装在鞋子上的 IMU 来接收位置数据。所以我发现我必须使用卡尔曼滤波器来获取有效的位置数据。

这就是我开始挣扎的地方。

大多数示例使用 6 DoF-IMU 使用卡尔曼滤波器计算位置。但我尝试使用 Invensense 的 9-DoF IMU ICM20948。它使用 Mag、Acc 和 Gyro 计算输出四元数。然后我用原始的 XYZ 加速度和给定的四元数计算北、东、下方向的加速度。

所以我对卡尔曼滤波器的输入只有北、东和下方向的加速度。我如何设置卡尔曼滤波器矩阵才能在这 3 个方向上获得有效的位置数据？

我根据我在网上找到的一个例子自己尝试了它，但它似乎不起作用，而且位置数据也不可信：

xt = [ 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 ]

zt = [ -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.

我实现了以下测量算法：

在此处输入图像描述

对于测量估计，我使用这个等式：

x(n) = x(n-1) + K*z

4

0 回答 0