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:

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.

I implemented following measurement algorithm:

For measurement estimate i use this equation:

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

Questions about inertial-navigation

Read more about inertial-navigation