6
$\begingroup$

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$.

$\endgroup$
  • 1
    $\begingroup$ 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. $\endgroup$ – Damien Dec 27 '14 at 1:07
  • 1
    $\begingroup$ 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. $\endgroup$ – Damien Dec 27 '14 at 1:10
  • $\begingroup$ Thank for your comment. Actually, eve with that, how to determine $Q$ and $R$? $\endgroup$ – jakeoung Dec 27 '14 at 7:06
  • $\begingroup$ You will need to post your process and measurement model (as $\LaTeX$, not code) before I can make an informed comment. $\endgroup$ – Damien Dec 27 '14 at 11:35
  • $\begingroup$ Okay, I've added equations. $\endgroup$ – jakeoung Dec 27 '14 at 12:56
1
$\begingroup$

R is depends on the sensor sensitivity. If this is a real world problem this can be obtained from the manufacturer. If not use the identity matrix multiplied by a scalar that is less than 1.

Q is the covariance of the process noise. Again if this is a real world problem this can be obtained in the noise level in the states of the system at steady state. if not you can assume Q is zero matrix. A non zero Q helps achieve good convergency characteristics as explained in: https://home.wlu.edu/~levys/kalman_tutorial/kalman_14.html

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.