tutorialpoint.org

# Linear Estimation

## 1. Kalman Filter Tutorial (Cont'd...)

### 1.4 Simulation

#### Kalman filter application for one dimensional motion

Let us consider a simple dynamic model of particle motion obeying the constant coefficient first order linear differential equation $$\label{5a} \dot{X}=FX(t)+G(t),$$ where, $X(t)=\begin{bmatrix} x(t) \\ \dot{x}(t) \\ \ddot{x}(t) \end{bmatrix}$, $F=\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix}$, $G(t)=\begin{bmatrix} 0 \\ 0 \\ g(t) \end{bmatrix}.$ The scalar quantity $x(t)$ is position, $\dot{x}(t)$ is velocity, $\ddot{x}(t)$ is acceleration, and $g(t)$ is the acceleration excitation at time $t$. If $g(t)=0$, then particle moving at constant velocity and if $g(t)\neq 0$, then changes occur in velocity. Now, we discretize the above equation to obtain $$\label{5b} X_{k}=\phi_{k,k-T}X_{k-1}+w_{k,\tau},$$ where, $\tau$ represents the time period, and the process noise is defined as, $$w_{k,\tau}=\int_{0}^{\tau}\phi_{k,\lambda}G_{\lambda}d\lambda,$$ and, transition matrix is defined as, $$\label{5c} \phi_{k,\lambda}=e^{(k-\lambda)F}=I+(k-\lambda)F+\dfrac{(k-\lambda)^{2}F^{2}}{2!}+\dfrac{(k-\lambda)^{3}F^{3}}{3!}+\dfrac{(k-\lambda)^{4}F^{4}}{4!}+.......,$$ where, $F=\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix}$, $F^{2}=\begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}$, and $F^{n}=\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}$ for all $n>2$ So, $$\label{5d} \phi_{k,\lambda}=I+(k-\lambda)F+\dfrac{(k-\lambda)^{2}F^{2}}{2!}=\begin{bmatrix} 1 & k-\lambda & \dfrac{(k-\lambda)^{2}}{2} \\ 0 & 1 & k-\lambda \\ 0 & 0 & 1 \end{bmatrix},$$ where, substituting the value of $\lambda=k-\tau$ into the above equation, we obtain $$\label{5e} \phi_{k,k-\tau}=I+\tau F+\dfrac{\tau^{2}F^{2}}{2}=\begin{bmatrix} 1 & \tau & \frac{1}{2}\tau^{2} \\ 0 & 1 & \tau \\ 0 & 0 & 1 \end{bmatrix}$$