Kalman for 3D position and 1D orientation

Question

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?

\begin{equation} 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} \end{equation}

---

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: \begin{equation} \pmb{\alpha}= \begin{bmatrix} 1 & dt & 0.5 \times dt^2 \\ 0 & 1 & dt \\ 0 & 0 & 1 \end{bmatrix}_{3 \times 3} \end{equation}

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

$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: \begin{equation} 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} \end{equation}

In the same manner, the transformation matrix $H$, which map state vector parameters into the measurements domain, is built using: \begin{equation} 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} \end{equation}

Answer 1

Reading your link in more detail, it says:

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.