1. Constant velocity

1.1. Stating the problem: movement with constant velocity.

Equation of motion: $$\dot{x} = const.$$ $$ x(t) = x_{0} + \dot{x}t$$

1.2. What's the state vector - we assume to be everything unknown, so everyhing but time:

$$ p(t) = \begin{bmatrix} x(t) \\ \dot{x} \end{bmatrix} $$

1.3. Measurement

1.3.1. Distance between measurement sites is expressed in terms of time: $\Delta t$. $$x(t_k) := x_k$$ $$x(t_k+\Delta t) := x_{k+1}$$ State vector at $t_k$ is given by $$p(t_k) := p_k := \begin{bmatrix} x_{k} \\ \dot{x}_{k} \end{bmatrix}$$

1.3.2. What we measure is a position at measurement sites. $$m_k = x_k = \begin{bmatrix} 1 && 0 \end{bmatrix} \cdot \begin{bmatrix} x_{k} \\ \dot{x}_{k} \end{bmatrix} = H \cdot p_k$$

$$ H = \begin{bmatrix} 1 && 0 \end{bmatrix} $$

1.4. Recursive equation of motion:

$$ \dot{x}_{k} = \dot{x}_{k-1}$$$$ x_{k} = x_{k-1} + \dot{x}_{k-1} \cdot \Delta t$$$$ p_{k} = \begin{bmatrix} x_{k} \\ \dot{x}_{k} \end{bmatrix} = \begin{bmatrix} x_{k-1} + \dot{x}_{k-1} \cdot \Delta t \\ \dot{x}_{k-1} \end{bmatrix} = \begin{bmatrix} 1 && \Delta t \\ 0 && 1 \end{bmatrix} \cdot \begin{bmatrix} x_{k-1} \\ \dot{x}_{k-1} \end{bmatrix} = A \cdot p_{k-1} $$$$ A = \begin{bmatrix} 1 && \Delta t \\ 0 && 1 \end{bmatrix} $$

1.5. Covariance matrices

1.5.1. Measurement accuracy (comes directly from the mechanism of measuring) $$V = 0.1~\mathrm{m}$$

1.5.2. Covariance matrix of state vector (initial!) Will be changing with time, we start with the largest value we could think of, so for instance we know the object total path is $20~\mathrm{m}$, so the maximum prediction error is the same. Coresponding maximum value for velocity can be assosiated with maximum position error and $\Delta t$. $$Q_0 = \begin{bmatrix} 20 && 0 \\ 0 && \frac{20}{\Delta t} \end{bmatrix}$$

WARNING: Manipulating covariance matrices affects badly the kalman efficiency! It's crucial to play with them to establish the best set.

1.6. Initial values

State vector: $$p_0 = \begin{bmatrix} m_{0} \\ \frac{m_1-m_0}{\Delta t} \end{bmatrix}$$

Covariance matrix $Q_0$ as explained above.

1.7. KALMAN - loop over all measurement sites

1.7.1. Prediction:

$$ \tilde{p}_k = A\cdot p_{k-1} $$

1.7.2. Update:

Compute Kalman's gain matrix: $$ Q_k = A\cdot Q_{k-1}*A^T $$ $$ K_k = (Q_k \cdot H^T) \cdot (H\cdot Q_k \cdot H^T + V)^{-1} $$

Now take into account the measurement:

• update of the state vector: $$ p_k = \tilde{p}_k + K_k \cdot (m_k - H\cdot \tilde{p}_k) $$

• update of the covariance matrix: $$Q_k = (I - K_k\cdot H ) \cdot Q_{k-1} $$

2. Constant acceleration

2.1. Stating the problem: movement with constant acceleration.

Equations of motion: $$\ddot{x} = const.$$ $$ \dot{x}(t) = \dot{x}_0 + \ddot{x}t$$ $$ x(t) = x_{0} + \dot{x}_0t + \ddot{x}t^2/2$$

2.2. What's the state vector - we assume to be everything unknown, so everyhing but time:

$$ p(t) = \begin{bmatrix} x(t) \\ \dot{x}(t) \\ \ddot{x} \end{bmatrix} $$

2.3. Measurement

2.3.1. Distance between measurement sites is expressed in terms of time: $\Delta t$. $$x(t_k) := x_k$$ $$x(t_k+\Delta t) := x_{k+1}$$ State vector at $t_k$ is given by $$p(t_k) := p_k := \begin{bmatrix} x_{k} \\ \dot{x}_{k} \\ \ddot{x}_{k} \end{bmatrix}$$

2.3.2. What we measure is a position at measurement sites. $$m_k = x_k = \begin{bmatrix} 1 && 0 && 0 \end{bmatrix} \cdot \begin{bmatrix} x_{k} \\ \dot{x}_{k} \\ \ddot{x}_{k} \end{bmatrix} = H \cdot p_k$$

$$ H = \begin{bmatrix} 1 && 0 && 0 \end{bmatrix} $$

2.4. Recursive equation of motion:

$$ \ddot{x}_{k} = \ddot{x}_{k-1}$$$$ \ddot{x}_{k} = \ddot{x}_{k-1} + \ddot{x}_{k-1} \cdot \Delta t$$$$ x_{k} = x_{k-1} + \dot{x}_{k-1} \cdot \Delta t + \ddot{x}_{k-1} \cdot \Delta t^2/2$$$$ p_{k} = \begin{bmatrix} x_{k} \\ \dot{x}_{k} \\ \ddot{x}_{k} \end{bmatrix} = \begin{bmatrix} x_{k-1} + \dot{x}_{k-1} \cdot \Delta t + \ddot{x}_{k-1} \cdot \Delta t^2/2 \\ \dot{x}_{k-1} + \ddot{x}_{k-1} \cdot \Delta t \\ \ddot{x}_{k-1} \end{bmatrix} = \begin{bmatrix} 1 && \Delta t && \Delta t^2/2 \\ 0 && 1 && \Delta t \\ 0 && 0 && 1\end{bmatrix} \cdot \begin{bmatrix} x_{k-1} \\ \dot{x}_{k-1} \end{bmatrix} = A \cdot p_{k-1} $$$$ A = \begin{bmatrix} 1 && \Delta t&& \Delta t^2/2\\ 0 && 1 && \Delta t \\ 0 && 0 && 1 \end{bmatrix} $$

2.5. Covariance matrices

2.5.1. Measurement accuracy (comes directly from the mechanism of measuring) $$V = 0.1~\mathrm{m}$$

2.5.2. Covariance matrix of state vector (initial!) Will be changing with time, we start with the largest value we could think of, so for instance we know the object total path is $20~\mathrm{m}$, so the maximum prediction error is the same. Coresponding maximum value for velocity can be assosiated with maximum position error and $\Delta t$. $$Q_0 = \begin{bmatrix} 20 && 0 && 0 \\ 0 && \frac{20}{\Delta t} && 0 \\ 0 && 0 && \frac{2\cdot 20}{\Delta t^2}\end{bmatrix}$$

WARNING: Manipulating covariance matrices affects badly the kalman efficiency! It's crucial to play with them to establish the best set.

2.6. Initial values

Estimation of initial acceleration: $$\ddot{x}_0 = \frac{\dot{x}_1-\dot{x}_0}{\Delta t} = \frac{\frac{m_1-m_0}{\Delta t} - \frac{m_2-m_1}{\Delta t}}{\Delta t} \approx 2\cdot\frac{m_1-m_0}{\Delta t^2} $$

State vector: $$p_0 = \begin{bmatrix} m_{0} \\ \frac{m_1-m_0}{\Delta t} \\ 2\cdot\frac{m_1-m_0}{\Delta t^2} \end{bmatrix}$$

Covariance matrix $Q_0$ as explained above.

2.7. KALMAN - loop over all measurement sites

2.7.1. Prediction:

$$ \tilde{p}_k = A\cdot p_{k-1} $$

2.7.2. Update:

Compute Kalman's gain matrix: $$ Q_k = A\cdot Q_{k-1}*A^T $$ $$ K_k = (Q_k \cdot H^T) \cdot (H\cdot Q_k \cdot H^T + V)^{-1} $$

Now take into account the measurement:

• update of the state vector: $$ p_k = \tilde{p}_k + K_k \cdot (m_k - H\cdot \tilde{p}_k) $$

• update of the covariance matrix: $$Q_k = (I - K_k\cdot H ) \cdot Q_{k-1} $$