# How to Determine Covariance Matrix $Q$ and $R$ in Kalman Filter

I am implementing getting orientation from smartphone. I want to use Kalman filter and should determine process noise covariance matrix $Q$ and measurement noise covariance matrix $R$. (newbie to Kalman filter)

I don't have any idea how to determine $Q$. What I think about $R$ is as follows:

state vector : quaternion from (accelerometer + gyroscope)

(1) My phone is stand still. I get covaraince matrix from Matlab

1.0e-04 *

0.0000    0.0005    0.0035   -0.0000
0.0005    0.0063    0.0411   -0.0002
0.0035    0.0411    0.2881   -0.0014


-0.0000 -0.0002 -0.0014 0.0000

(2) My phone had been moved for 5 seconds.

covariance matrix is

0.0417   -0.0533   -0.0008   -0.0014
-0.0533    0.0784    0.0015    0.0018
-0.0008    0.0015    0.0001    0.0001
-0.0014    0.0018    0.0001    0.0001


Is there anyone to help?

Details are omitted.

# case 1: Kalman Filter

The row data from my phone is $p, q, r$ (angular velocity). I omit the conversion equation between angular velocity and quaternion. \begin{align*} x_{k+1} &= Ax_k+w_k \\ z_k &= Hx_k + \nu_k \\ Q &: \text{ covariance matrix for }w_k\\ R &: \text{ covariance matrix for }\nu_k \end{align*}

$$\begin{bmatrix}\dot q_1\\\dot q_2\\\dot q_3\\\dot q_4\end{bmatrix}=\frac 12 \begin{bmatrix} 0&-p&-q&-r\\p&0&r&-q\\q&-r&0&p\\r&q&-p&0 \end{bmatrix} \begin{bmatrix} q_1\\ q_2\\ q_3\\ q_4\end{bmatrix}$$

$$\underbrace{\begin{bmatrix} q_1\\ q_2\\ q_3\\ q_4\end{bmatrix}_{k+1}}_{x_{k+1}}=\underbrace{\left( I + \Delta t\cdot \frac 12 \begin{bmatrix} 0&-p&-q&-r\\p&0&r&-q\\q&-r&0&p\\r&q&-p&0 \end{bmatrix}\right)}_{A} \underbrace{\begin{bmatrix} q_1\\ q_2\\ q_3\\ q_4\end{bmatrix}_k}_{x_k}$$ $$H=I$$ My guess for covariance matrix is as follows: (but I don' know how to infer..) $$Q = 0.001I, \quad R=10I.$$

# case 2: Extended Kalman Filter

\begin{align*}x_{k+1} &= f(x_k) + w_k \\ z_k &= h(x_k) + \nu_k\\ Q &: \text{ covariance matrix for }w_k\\ R &: \text{ covariance matrix for }\nu_k \end{align*} $$A = \left.\frac{\partial f}{\partial x}\right|_{x_k} ,\quad H = \left.\frac{\partial h}{\partial x}\right|_{x_k}$$ \begin{align*} \begin{bmatrix} \dot \phi\\ \dot \theta\\ \dot \varphi \end{bmatrix}&= \begin{bmatrix} 1&\sin\phi\tan\theta & \cos\phi\tan\theta \\ 0&\cos\phi & -\sin\phi \\ 0&\sin\phi\sec\theta & \cos\phi\sec\theta \end{bmatrix} \begin{bmatrix}p\\ q\\ r \end{bmatrix} \\ &= \begin{bmatrix} p+q\sin\phi\tan\theta+r\cos\phi\tan\theta \\ q\cos\phi-r\sin\phi \\ q\sin\phi\sec\theta + r\cos\phi\sec\theta \end{bmatrix} \\ &= f(x) + w \end{align*} $$z = \begin{bmatrix}1&0&0\\0&1&0\end{bmatrix}\begin{bmatrix}\phi\\\theta\\\varphi\end{bmatrix}+\nu = Hx + \nu$$ $$A = \begin{bmatrix} \frac{\partial f_1}{\partial\phi} & \frac{\partial f_1}{\partial\theta} & \frac{\partial f_1}{\partial\varphi} \\ \frac{\partial f_2}{\partial\phi} & \frac{\partial f_2}{\partial\theta} & \frac{\partial f_2}{\partial\varphi} \\ \frac{\partial f_3}{\partial\phi} & \frac{\partial f_3}{\partial\theta} & \frac{\partial f_3}{\partial\varphi} \end{bmatrix}$$

(I emphasize that details are omitted.) In this case, also I don't know how to infere $Q, R$.

• The direct use of a quaternion in a Kalman Filter is bad news - a quaternion is not a vector and the "states" are not independent, which essentially destroys the assumptions of the filter. Accordingly, the covariance is meaningless. Dec 27, 2014 at 1:07
• Instead, formulate the filter in terms of error states, or if you insist on using direct attitude terms, use an Extended Kalman Filter with Euler Angles. There's some serious maths here, but textbooks from Groves and Farrell are quite useful. Dec 27, 2014 at 1:10
• Thank for your comment. Actually, eve with that, how to determine $Q$ and $R$? Dec 27, 2014 at 7:06
• You will need to post your process and measurement model (as $\LaTeX$, not code) before I can make an informed comment. Dec 27, 2014 at 11:35
• Okay, I've added equations. Dec 27, 2014 at 12:56