Position 仅加速跟踪

Question

i am working on a Project where i want to receive position data by using an IMU mounted on a shoe. So i found out that i have to use a kalmanfilter to get valid position data.

And thats the point where i begin to struggle.

The most examples use a 6 DoF-IMU to calculate the position using a Kalman Filter. But i tried to use the 9-DoF IMU ICM20948 from Invensense. It calculates with Mag, Acc and Gyro to out put the Quaternion. Then i calculate the Acceleration in North,East,Down-Direction with the raw XYZ-Acceleration and the given Quaternions.

So my input for the Kalman Filter ist only the Acceleration in North,East and Down direction. How do i have to set the Kalman Filter-Matrices to get valid Position-Data in these 3 directions?

I tried it on my own based on a example i found online but it doesnt seem to work and the position data is not plausible:

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.

I implemented following measurement algorithm:

Answer 1

I just use a different measurement update equation:

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