Project - Kalman Filter (Discard)

Haiyue10/29/23About 10 min

What is Kalma Filter

The Kalman Filter is a mathematical algorithm used for estimating and predicting the state of a dynamic system, particularly in the presence of noisy or uncertain data. It consist of five equations.

Predict State

Predict State:
${\hat{x}}_{n + 1, n} = F {\hat{x}}_{n, n} + G u_{n} + ω_{n}$
Predict Noise:
${\hat{P}}_{n + 1, n} = F {\hat{P}}_{n, n} F^{T} + Q$

Update State

Kalman Gain:
$K_{n} = P_{n, n - 1} H^{T} (H P_{n, n - 1} H^{T} + R_{n})^{- 1}$
Update State:
${\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + K_{n} (z_{n} - H {\hat{x}}_{n, n - 1})$
Update Noise:
$P_{n, n} = (I - K_{n} H) P_{n, n - 1} (I - K_{n} H)^{T} + K_{n} R_{n} K_{n}^{T}$

STATE EXTRAPOLATION EQUATION

${\hat{x}}_{n + 1, n} = F {\hat{x}}_{n, n} + G u_{n} + ω_{n}$

Item	Mean
${\hat{x}}_{n + 1, n}$	predicted system state vector at time step $n + 1$
${\hat{x}}_{n, n}$	estimated system state vector at time step $n$
$u_{n}$	a control variable or input variable - a measurable (deterministic) input to the system
$w_{n}$	a process noise or disturbance - an unmeasurable input that affects the state
$F$	a state transition matrix
$G$	a control matrix or input transition matrix (mapping control to state variables)

Prediction

${\hat{x}}^{-} = F {\hat{x}}_{t - 1} + B u_{t - 1}$
$P_{t}^{-} = F P_{t - 1} F^{T} + Q$

Update

${\hat{x}}_{t} = {\hat{x}}_{t}^{-} + K_{t} (Z_{t} - H {\hat{x}}_{t}^{-})$
$K_{t} = P_{t}^{-} H^{T} (H P_{t}^{-} H^{T} + R)^{- 1}$
$P_{t} = (I - K_{t} H) P_{t}^{-}$
Get the best estimation using the measurment.
Kalman gamin.
Update the noise distribution of best estimates.

$K_{t}$ : Kalman gain
$R$ : Observation covariance

Important basic concepts

Mean $μ = \frac{1}{N} \sum_{n = 1}^{N} V_{n}$
Expectation: The mean of multiple measurements， similar with mean
Variance: $σ^{2} = \frac{1}{N} \sum_{n = 1}^{N} (V_{n} - μ)^{2}$
Standard Variance: $σ$
Normal Distribution (All called Gaussian Distribution)
$f (x; μ, σ^{2}) = \frac{1}{\sqrt{2 π σ^{2}}} e^{\frac{- (x - μ)^{2}}{2 σ^{2}}}$

Typically, measurement errors are normally distributed. The Kalman filter assumes that the measurement errors have a normal distribution.
Estimation: An estimate of the hidden state of the system.
Accuracy: Describes how close a measured value is to the true value.
Precision: Describes the deviation distribution of a series of measured values relative to the same true value
Info
High-precision systems have measurements with very low variance (i.e., low uncertainty), while low-precision systems have high variance (i.e., high uncertainty).

Info

Typically, measurement errors are normally distributed. The Kalman filter assumes that the measurement errors have a normal distribution.

Example

How to measure a gold

How to weight a $x$ gold.

Info

$x$ : The true weight of the gold.
$z_{n}$ : The measurement of gold.
${\hat{x}}_{n, n}$ : The estimation of $x$ using the measurement at n moment.
${\hat{x}}_{n + 1, n}$ : It is the prediction of the future state (time $n + 1$ ) at time $n$ , recorded as $x_{n + 1, n}$ , or extrapolation.
${\hat{x}}_{n - 1, n - 1}$ : The estimated value of $x$ at time $n - 1$ , using the measured value $z_{n - 1}$ at time $n - 1$ .
${\hat{x}}_{n, n - 1}$ : Apriori estimate - a prediction of the system state at time n at time n−1 (Annotation: for the $n^{t h}$ time, $x_{n, n - 1}$ is a priori estimate, $x_{n + 1, n}$ is prediction)

In order to estimate $x_{n, n}$ , we need to store all historical measurements, which is very expensive on memory. And every time a new measurement value is obtained, the calculation needs to be completely restarted from the first measurement, which also consumes a huge amount of CPU computing power.
A more realistic consideration is that it is best to only store the estimated value $x_{n - 1, n - 1}$ of the previous moment, and simply update it after new measurements are completed.
Schematic Diagram

Formula	Meaning
${\hat{x}}_{n, n} = \frac{1}{n} \sum_{i = 1}^{n} (Z_{i})$	The mean of measurement
$= \frac{1}{n} (\sum_{i = 1}^{n - 1} (Z_{i}) + Z_{n})$
$= \frac{1}{n} \sum_{i = 1}^{n - 1} (Z_{i}) + \frac{1}{n} Z_{n}$
$= \frac{1}{n} \frac{n - 1}{n - 1} \sum_{i = 1}^{n - 1} (Z_{i}) + \frac{1}{n} Z_{n}$
$= \frac{n - 1}{n} \frac{1}{n - 1} \sum_{i = 1}^{n - 1} (Z_{i}) + \frac{1}{n} Z_{n})$	The orange item is the estimation at time n-1
$= \frac{n - 1}{n} {\hat{x}}_{n - 1, n - 1} + \frac{1}{n} Z_{n})$
$= {\hat{x}}_{n - 1, n - 1} - \frac{1}{n} {\hat{x}}_{n - 1, n - 1} + \frac{1}{n} Z_{n}$
$= {\hat{x}}_{n - 1, n - 1} - \frac{1}{n} ({\hat{x}}_{n - 1, n - 1} + Z_{n})$	The final formula

The final formula is the status update equation. One of five equations of Kalman filter.

Track constant velocity

$v = \frac{Δ x}{Δ t}$
$x_{n + 1} = x_{n} + Δ t * v_{n}$
$v_{n + 1} = v_{n}$

How to track a Car

Position: $p_{t} = p_{t - 1} + v_{t - 1} Δ t + u_{t} \frac{Δ t^{2}}{2}$
Speed: $v_{t} = v_{t - 1} + u_{t} Δ t$

The tow formula could be transfored into
$[\begin{matrix} p_{t} \\ v_{t} \end{matrix}] = [\begin{matrix} 1 & Δ t \\ 0 & 1 \end{matrix}] [\begin{matrix} p_{t - 1} \\ v_{t - 1} \end{matrix}] + [\begin{matrix} \frac{Δ t^{2}}{2} \\ Δ t \end{matrix}] u_{t}$

$F_{t} = [\begin{matrix} 1 & Δ t \\ 0 & 1 \end{matrix}], B_{t} = [\begin{matrix} \frac{Δ t^{2}}{2} \\ Δ t \end{matrix}] u_{t}$

Then the formulas could be transformed into
${\hat{x}}^{-} = F {\hat{x}}_{t - 1} + B u_{t - 1}$

How to track a plane

If we want to track a plane, how to do it.
The state will be
$x = x_{0} + v_{0} Δ t + \frac{1}{2} a Δ t^{2}$

Info

$x$ : is the position of a plan
$x_{0}$ : the initial postion
$v_{0}$ : the initial speed
$a$ : The acceleration
$Δ t$ : Sample period

Because of we location a 3 dimentions world. There are 3 components in 3 different direction.

$x = {\begin{cases} x = x_{0} + v_{x} Δ t + \frac{1}{2} a_{x} Δ t^{2} \\ y = y_{0} + v_{y} Δ t + \frac{1}{2} a_{y} Δ t^{2} \\ z = z_{0} + v_{z} Δ t + \frac{1}{2} a_{z} Δ t^{2} \end{cases}$

So the state in three world would be like below.
$[\begin{matrix} x \\ y \\ y \\ v_{x} \\ v_{y} \\ v_{z} \\ a_{x} \\ a_{y} \\ a_{z} \end{matrix}]$
$[\begin{matrix} v_{x} = \dot{x} \\ v_{y} = \dot{y} \\ v_{z} = \dot{z} \\ a_{x} = \ddot{x} \\ a_{y} = \ddot{y} \\ a_{z} = \ddot{z} \end{matrix}]$

${\begin{cases} P o s i t i o n_{n} & = P o s i t i o n_{(n - 1)} + V e l o c i t y_{(n - 1)} Δ t + \frac{1}{2} A c c e l e r a t e_{(n - 1)} Δ t^{2} \\ V e l o c i t y_{n} & = V e l o c i t y_{(n - 1)} + A c c e l e r a t e_{(n - 1)} Δ t \\ A c c e l e r a t e_{n} & = A c c e l e r a t e_{(n + 1)} \end{cases}$

$[\begin{matrix} x \\ y \\ y \\ \dot{x} \\ \dot{y} \\ \dot{z} \\ \ddot{x} \\ \ddot{y} \\ \ddot{z} \end{matrix}]$

$[\begin{matrix} p o s i t i o n \\ v e l o c i t y \\ a c c e l e r a t i o n \end{matrix}]$

Predict State

Predict State:
${\hat{x}}_{n + 1, n} = F {\hat{x}}_{n, n} + G u_{n} + ω_{n}$
Predict Noise:
${\hat{P}}_{n + 1, n} = F {\hat{P}}_{n, n} F^{T} + Q$

Update States

${\begin{cases} x_{n} = x_{n - 1} + v_{n - 1} Δ t + \frac{1}{2} a_{n - 1} Δ t^{2} \\ y_{n} = y_{n - 1} + v_{n - 1} Δ t + \frac{1}{2} a_{n - 1} Δ t^{2} \\ z_{n} = z_{n - 1} + v_{n - 1} Δ t + \frac{1}{2} a_{n - 1} Δ t^{2} \\ {\dot{x}}_{n} = {\dot{x}}_{n - 1} + {\ddot{x}}_{n - 1} Δ t \\ {\dot{y}}_{n} = {\dot{y}}_{n - 1} + {\ddot{7}}_{n - 1} Δ t \\ {\dot{z}}_{n} = {\dot{z}}_{n - 1} + {\ddot{z}}_{n - 1} Δ t \\ {\ddot{x}}_{n} = {\ddot{x}}_{n - 1} \\ {\ddot{y}}_{n} = {\ddot{y}}_{n - 1} \\ {\ddot{z}}_{n} = {\ddot{z}}_{n - 1} \end{cases} = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} \\ 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{(n - 1)} \\ y_{(n - 1)} \\ z_{(n - 1)} \\ {\dot{x}}_{(n - 1)} \\ {\dot{y}}_{(n - 1)} \\ {\dot{z}}_{(n - 1)} \\ {\ddot{x}}_{(n - 1)} \\ {\ddot{y}}_{(n - 1)} \\ {\ddot{z}}_{(n - 1)} \end{matrix}]$

The formula could transform to state prediction. We will set the control vector and transition matrix like below.

The control vector $u_{n}$ :
$u_{n} = [\begin{matrix} {\ddot{x}}_{n} \\ {\ddot{y}}_{n} \\ {\ddot{z}}_{n} \end{matrix}]$

The state transition matrix $F$ :
$F = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]$

The control matrix $G$ :
$G = [\begin{matrix} 0.5 Δ t^{2} & 0 & 0 \\ 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 0.5 Δ t^{2} \\ Δ t & 0 & 0 \\ 0 & Δ t & 0 \\ 0 & 0 & Δ t \end{matrix}]$

Thus the state prediction formula ${\hat{x}}_{n + 1, n} = F {\hat{x}}_{n, n} + G u_{n}$ will transform like below.
${\hat{x}}_{n + 1, n} = [\begin{matrix} x_{(n + 1, n)} \\ y_{(n + 1, n)} \\ z_{(n + 1, n)} \\ {\dot{x}}_{(n + 1, n)} \\ {\dot{y}}_{(n + 1, n)} \\ {\dot{z}}_{(n + 1, n)} \end{matrix}] = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{(n, n)} \\ y_{(n, n)} \\ z_{(n, n)} \\ {\dot{x}}_{(n, n)} \\ {\dot{y}}_{(n, n)} \\ {\dot{z}}_{(n, n)} \end{matrix}] + [\begin{matrix} 0.5 Δ t^{2} & 0 & 0 \\ 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 0.5 Δ t^{2} \\ Δ t & 0 & 0 \\ 0 & Δ t & 0 \\ 0 & 0 & Δ t \end{matrix}] [\begin{matrix} {\ddot{x}}_{n} \\ {\ddot{y}}_{n} \\ {\ddot{z}}_{n} \end{matrix}]$

Airplane without control input

Consider an airplane moving in three-dimensional space with constant acceleration.

The state vector ${\hat{x}}_{n} = [\begin{matrix} x \\ y \\ y \\ \dot{x} \\ \dot{y} \\ \dot{z} \\ \ddot{x} \\ \ddot{y} \\ \ddot{z} \end{matrix}]$

The state transition matrix $F = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} \\ 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]$

The state extrapolation equation is:
$[\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} \\ 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{(n - 1)} \\ y_{(n - 1)} \\ z_{(n - 1)} \\ {\dot{x}}_{(n - 1)} \\ {\dot{y}}_{(n - 1)} \\ {\dot{z}}_{(n - 1)} \\ {\ddot{x}}_{(n - 1)} \\ {\ddot{y}}_{(n - 1)} \\ {\ddot{z}}_{(n - 1)} \end{matrix}] \to {\begin{cases} x_{n} = x_{n - 1} + v_{n - 1} Δ t + \frac{1}{2} a_{n - 1} Δ t^{2} \\ y_{n} = y_{n - 1} + v_{n - 1} Δ t + \frac{1}{2} a_{n - 1} Δ t^{2} \\ z_{n} = z_{n - 1} + v_{n - 1} Δ t + \frac{1}{2} a_{n - 1} Δ t^{2} \\ {\dot{x}}_{n} = {\dot{x}}_{n - 1} + {\ddot{x}}_{n - 1} Δ t \\ {\dot{y}}_{n} = {\dot{y}}_{n - 1} + {\ddot{7}}_{n - 1} Δ t \\ {\dot{z}}_{n} = {\dot{z}}_{n - 1} + {\ddot{z}}_{n - 1} Δ t \\ {\ddot{x}}_{n} = {\ddot{x}}_{n - 1} \\ {\ddot{y}}_{n} = {\ddot{y}}_{n - 1} \\ {\ddot{z}}_{n} = {\ddot{z}}_{n - 1} \end{cases}$

Airplane with control input

he state vector ${\hat{x}}_{n}$ that describes the estimated airplane position and velocity in a cartesian coordinate system like below.

${\hat{x}}_{n} = {\begin{cases} {\hat{x}}_{n} \\ {\hat{y}}_{n} \\ {\hat{z}}_{n} \\ {\hat{\dot{x}}}_{n} \\ {\hat{\dot{y}}}_{n} \\ {\hat{\dot{z}}}_{n} \end{cases}$

The control vector $u_{n}$ that describes the measured airplane acceleration in a cartesian coordinate system
$u_{n} = [\begin{matrix} {\ddot{x}}_{n} \\ {\ddot{y}}_{n} \\ {\ddot{z}}_{n} \end{matrix}]$

The state transition matrix: $F = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]$

The control matrix: $G = [\begin{matrix} 0.5 Δ t^{2} & 0 & 0 \\ 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 0.5 Δ t^{2} \\ Δ t & 0 & 0 \\ 0 & Δ t & 0 \\ 0 & 0 & Δ t \end{matrix}]$

Thus the state prediction formula ${\hat{x}}_{n + 1, n} = F {\hat{x}}_{n, n} + G u_{n}$ will transform like below.

${\hat{x}}_{n + 1, n} = [\begin{matrix} x_{(n + 1, n)} \\ y_{(n + 1, n)} \\ z_{(n + 1, n)} \\ {\dot{x}}_{(n + 1, n)} \\ {\dot{y}}_{(n + 1, n)} \\ {\dot{z}}_{(n + 1, n)} \end{matrix}] = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{(n, n)} \\ y_{(n, n)} \\ z_{(n, n)} \\ {\dot{x}}_{(n, n)} \\ {\dot{y}}_{(n, n)} \\ {\dot{z}}_{(n, n)} \end{matrix}] + [\begin{matrix} 0.5 Δ t^{2} & 0 & 0 \\ 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 0.5 Δ t^{2} \\ Δ t & 0 & 0 \\ 0 & Δ t & 0 \\ 0 & 0 & Δ t \end{matrix}] [\begin{matrix} {\ddot{x}}_{n} \\ {\ddot{y}}_{n} \\ {\ddot{z}}_{n} \end{matrix}]$