# Kalman filter to calculate quaternion using gyroscope and accelerometer

I have a 6 DOF imu and i am trying to implement an extended kalman filter to calculate the quaternion. Where I use the gyroscope in the prediction step and the accelerometer as the update step.

The F matrix using the gyroscope was not so hard to understand. But I can not figure my H matrix. any help ?

Hmm I'm not sure I currently have all the elements to answer, but here is a similar approach I used:

So, let Dt be the time between the IMU measurements at time K and K-1.

Imu_force is the IMU specific force data (given in vehicle frame).

Imu_w is the IMU rotational velocity (given in vehicle frame)

C_ns is the estimate of the quaternion at time K-1.

G is the gravity (used here to counterbalance the data from the IMU)

Let p_est be the position estimation

Let v_est be the speed estimation

Let q_est be the quaternion estimation

$$P_k = P_{k-1} + Dt * v_{est_{k-1}} + ((Dt ^ 2) / 2) * ((C_{ns} * Imuforce_{k-1}) + G)$$

$$V_{est_k} = V_{est_{[k-1]}} + Dt * (C_{ns} * Imuforce_{k-1} + G)$$

$$Q_{est_k} = Imu_{w{k-1}} * Dt * Q_{est_{k-1}}$$ $$Fk = \begin{matrix} 1 & 0 & 0 & Dt & 0 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & Dt & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & Dt & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0 & C_ns . skew(imuforce[k-1]*Dt) & C_ns . skew(imuforce[k-1]*Dt) & C_ns . skew(imuforce[k-1]*Dt)\\ 0 & 0 & 0 & 0 & 1 & 0 & C_ns . skew(imuforce[k-1]*Dt) & C_ns . skew(imuforce[k-1]*Dt) & C_ns . skew(imuforce[k-1]*Dt)\\ 0 & 0 & 0 & 0 & 0 & 1 & C_ns . skew(imuforce[k-1]*Dt) & C_ns . skew(imuforce[k-1]*Dt) & C_ns . skew(imuforce[k-1]*Dt)\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix}$$

$$Q_k = Dt^2 * \begin{matrix} varIMUf & 0 & 0 & 0 & 0 & 0 \\ 0 & varIMUf & 0 & 0 & 0 & 0 \\ 0 & 0 & varIMUf & 0 & 0 & 0 \\ 0 & 0 & 0 & varIMUw & 0 & 0 \\ 0 & 0 & 0 & 0 & varIMUw & 0 \\ 0 & 0 & 0 & 0 & 0 & varIMUw \\ \end{matrix}$$

$$Lk = \begin{matrix} 1& 0 & 0 & 0 & 0 & 0 \\ 0 & 1& 0 & 0 & 0 & 0 \\ 0 & 0 & 1& 0 & 0 & 0 \\ 0 & 0 & 0 & 1& 0 & 0 \\ 0 & 0 & 0 & 0 & 1& 0 \\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix}$$

$$P_{cov} = F_k * P_{k-1} * transpose(F_k) + L_k * Q_k * transpose(L_k)$$

With all this you would have your motion model. Then what I did is I updated using a position update.

In this frame, my H matrix jacobian for the measurement (updating the model with a new position) was:

$$H = \begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 &0 &0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 &0 &0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 &0 &0 \end{matrix}$$

I hope this helps somehow.