# Kalman for 3D position and 1D orientation

Is the following state correct for a Constant Acceleration (CA) model KF applied for tracking an object which moves in (x, y, z) and have the freedom of rotation $\phi$ around the $z$ axis?

$$x= \begin{bmatrix} x, \dot x, \ddot x, y, \dot y, \ddot y, z, \dot z, \ddot z, \phi, \dot \phi, \ddot \phi \end{bmatrix}$$

UPDATE:

I can only measure $(x, y, z)$, directly. After, I computed $\phi$, rotation around the $z$ axis, using $\arctan(\frac{\delta y}{\delta x})$, indirectly. The measurements that I input to KF are: $x, y, z, \phi$.

Assuming: $$\pmb{\alpha}= \begin{bmatrix} 1 & dt & 0.5 \times dt^2 \\ 0 & 1 & dt \\ 0 & 0 & 1 \end{bmatrix}_{3 \times 3}$$

$$\pmb{\beta}= \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}_{1 \times 3}$$

$A$, the state transition matrix which applies the effect of each state parameter at time $t-1$ on the state at time $t$, is built using: $$x_{k+1}= \begin{bmatrix} \pmb{\alpha}_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & \pmb{\alpha}_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & \pmb{\alpha}_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & \pmb{\alpha}_{3 \times 3} \\ \end{bmatrix}_{12 \times 12} \cdot \begin{bmatrix} x\\ \dot x\\ \ddot x\\ y\\ \dot y\\ \ddot y\\ z\\ \dot z\\ \ddot z\\ \phi\\ \dot \phi\\ \ddot \phi \end{bmatrix}_{k}$$

In the same manner, the transformation matrix $H$, which map state vector parameters into the measurements domain, is built using: $$H= \begin{bmatrix} \pmb{\beta}_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} \\ 0_{1 \times 3} & \pmb{\beta}_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} \\ 0_{1 \times 3} & 0_{1 \times 3} & \pmb{\beta}_{1 \times 3} & 0_{1 \times 3} \\ 0_{1 \times 3} & 0_{1 \times 3} & 0_{1 \times 3} & \pmb{\beta}_{1 \times 3} \\ \end{bmatrix}_{4 \times 12}$$

• Welcome to DSP.SE! If the object has constant acceleration, why does the acceleration ($\ddot{x}, \ddot{y}, \ddot{z}$) need to be in the state? It can be, I'm just wondering whether it needs to be.
– Peter K.
May 9, 2016 at 15:02
• nbviewer.jupyter.org/github/balzer82/Kalman/blob/master/… May 10, 2016 at 16:53
• Ok, following that as an example, then your state looks good.
– Peter K.
May 10, 2016 at 17:21
• Thanks Peter, but I'm still wondering about your question. In constant acceleration model, the acceleration need to be in the state or not? May 10, 2016 at 17:51

Acceleration ($\ddot{x}$ & $\ddot{y}$) as well as position ($x$ & $y$) is measured.

which, to me, is not a constant acceleration. What it is doing is a discretized changing acceleration model, assuming that the acceleration is constant between each sample period.

Now to your question: is inclusion of $\phi, \dot \phi,$ and $\ddot \phi$ in the state vector valid?

The answer is yes, but you need to include the dynamics of how those states interact with the other states.

OK, so now you've added the extra information, I can take a stab at it.

It should work with the following provisos:

• The noise distribution on the angle $\phi$ will be uniform not Gaussian. I can't find a good reference, but this discussion on comp.dsp mentions it. That means the KF equations for $\phi$ won't be quite right. I don't think this is a big deal.

• There will be some coloration between the noise on $\phi$ and the noises on $x$ and $y$. I'm not sure what this should be, but it may mean you need to introduce some off-diagonal terms in the $Q$ matrix in the KF formulation.

• Please see $A$ and $H$ above. May 11, 2016 at 16:07
• question updated! May 11, 2016 at 16:38