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.