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?
(added)
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$.
