8

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

Royi
  • 19,608
  • 4
  • 197
  • 238
jakeoung
  • 457
  • 1
  • 6
  • 17
  • 1
    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. – Damien Dec 27 '14 at 01:07
  • 1
    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. – Damien Dec 27 '14 at 01:10
  • Thank for your comment. Actually, eve with that, how to determine $Q$ and $R$? – jakeoung Dec 27 '14 at 07:06
  • You will need to post your process and measurement model (as $\LaTeX$, not code) before I can make an informed comment. – Damien Dec 27 '14 at 11:35
  • Okay, I've added equations. – jakeoung Dec 27 '14 at 12:56
  • Case 1 cannot be used as a Kalman Filter - the quaternion is not a vector - the components are not independent. There are ways around that but the techniques are quite advanced. – Damien Jan 01 '15 at 07:16
  • Let us work with case 2 then. Can you identify where the gyro and accelerometer measurements are taken into account? – Damien Jan 01 '15 at 07:17
  • Why is the quaternion not a vector? I used Case 1 and got a reasonable result. What is the problem? – jakeoung Jan 01 '15 at 12:19
  • I got accelerometr values from which I got euler angle $\phi, \theta$. Then run EKF whose parameters are $\phi, \theta$, p, q, r, dt where p,q,r are measured gyroscope values. – jakeoung Jan 01 '15 at 12:27
  • "Why is the quaternion not a vector?" Because adding two (unit) quaternions does not give a unit quaternion. Accordingly, the KF update equation, $x(k+1|k+1) = x(k+1|k) + W (z(k+1) - z(k+1|k+1))$ Is all but guaranteed to fall over. – Damien Jan 01 '15 at 23:46
  • On Case 2: Are your equations in discrete time, as required by the KF family? – Damien Jan 01 '15 at 23:48
  • On Case 1: Is it true that if something is not a vector, it cannot be a state variable of Kalman filter? – jakeoung Jan 02 '15 at 10:52
  • On Case 2: I don't know exactly 'KF family', but equations are in discrete time. – jakeoung Jan 02 '15 at 10:54
  • On Case 1: For optimality of the KF, the states must belong to vector space. Of course, engineers have to make things work and sometimes have to accept a sub-optimal solution. If it looks close enough to a vector, where the states can be approximately added and scaled, then a KF can often give adequate performance. In this instance, an example is a attitude error state, which is a nominal deviation from a reference attitude. See references above for details. – Damien Jan 03 '15 at 00:20
  • On Case 1 (continued): If the states cannot be added to anything meaningful, then a KF cannot be used, except using some advanced (and nasty) tricks. This is exactly the case for attitude quaternions - clearly (as mentioned above) the state update is completely meaningless. – Damien Jan 03 '15 at 00:23
  • On Case 2: Your state transition equations as listed are in continuous time. Next hint: your $[p, q, r]^T$ can be considered a control input. Next hint after that: Consider your gyros to have white noise in addition to your actual measurements. What happens? – Damien Jan 03 '15 at 00:26
  • Thank you for your comment. I will check it. As a newbie for kalman filter, it is confusing. Following your comment, my book has an error. – jakeoung Jan 03 '15 at 04:09
  • On case 2, implementation is in discrete time. For the simple description, I omit implementation details. Thanks. – jakeoung Jan 03 '15 at 04:09
  • Let us continue this discussion in chat. – Damien Jan 03 '15 at 09:41
  • Please see the reports arXiv:1503.04313v1.pdf, arXiv:1505.07201v1.pdf and arXiv:1505.07208v1.pdf. and the papers in SADHANA December 2016 issues. – ANANTHASAYANAM Apr 20 '17 at 08:55

1 Answers1

2

R 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 here.

Peter K.
  • 25,714
  • 9
  • 46
  • 91
Engin Atik
  • 21
  • 2