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 ?
 A: 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.
