navigation - 加速度のみの位置追跡

Question

私は、靴に取り付けられた IMU を使用して位置データを受信したいプロジェクトに取り組んでいます。そのため、有効な位置データを取得するには kalmanfilter を使用する必要があることがわかりました。

そして、それが私が苦労し始めるポイントです。

ほとんどの例では、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.

次の測定アルゴリズムを実装しました。