My presentation slides
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: x ^ n + 1 , n = F x ^ n , n + G u n + ω n \hat{x}_{n+1,n} = F\hat{x}_{n,n} + Gu_n + \omega_n x ^ n + 1 , n = F x ^ n , n + G u n + ω n Predict Noise: P ^ n + 1 , n = F P ^ n , n F T + Q \hat{P}_{n+1,n} = F\hat{P}_{n,n}F^T + Q P ^ n + 1 , n = F 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 K_n = P_{n, n-1}H^T(HP_{n,n-1}H^T + R_n)^{-1} K n = P n , n − 1 H T ( H P n , n − 1 H T + R n ) − 1 Update State: x ^ n , n = x ^ n , n − 1 + K n ( z n − H x ^ n , n − 1 ) \hat{x}_{n,n} = \hat{x}_{n,n-1} + K_n(z_n-H\hat{x}_{n,n-1}) x ^ n , n = x ^ n , n − 1 + K n ( z n − H 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 P_{n,n} = (I-K_nH)P_{n,n-1}(I-K_nH)^T + K_nR_nK_n^T P n , n = ( I − K n H ) P n , n − 1 ( I − K n H ) T + K n R n K n T
x ^ n + 1 , n = F x ^ n , n + G u n + ω n \hat{x}_{n+1,n} = F\hat{x}_{n,n} + Gu_n + \omega_n x ^ n + 1 , n = F x ^ n , n + G u n + ω n
Item Mean x ^ n + 1 , n \hat{x}_{n+1,n} x ^ n + 1 , n predicted system state vector at time step n + 1 n+1 n + 1 x ^ n , n \hat{x}_{n,n} x ^ n , n estimated system state vector at time step n n n u n u_n u n a control variable or input variable - a measurable (deterministic) input to the system w n w_n w n a process noise or disturbance - an unmeasurable input that affects the state F F F a state transition matrix G G G a control matrix or input transition matrix (mapping control to state variables)
x ^ − = F x ^ t − 1 + B u t − 1 \hat x^- = F\hat x_{t-1} + Bu_{t-1} x ^ − = F x ^ t − 1 + B u t − 1 P t − = F P t − 1 F T + Q P_t^- = FP_{t-1}F^T+Q P t − = F P t − 1 F T + Q x ^ t = x ^ t − + K t ( Z t − H x ^ t − ) \hat x_t = \hat x_t^- + K_t(Z_t - H\hat x_t^-) x ^ t = x ^ t − + K t ( Z t − H x ^ t − )
K t = P t − H T ( H P t − H T + R ) − 1 K_t = P_t^-H^T(HP_t^-H^T+R)^{-1} K t = P t − H T ( H P t − H T + R ) − 1
P t = ( I − K t H ) P t − P_t=(I - K_tH)P_t^- 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 K_t K t R R R
Mean μ = 1 N ∑ n = 1 N V n \mu = \frac{1}{N}\displaystyle\sum_{n=1}^{N}V_n μ = N 1 n = 1 ∑ N V n Expectation: The mean of multiple measurements， similar with mean Variance: σ 2 = 1 N ∑ n = 1 N ( V n − μ ) 2 \sigma^2 = \frac{1}{N} \displaystyle\sum_{n=1}^{N}(V_n-\mu)^2 σ 2 = N 1 n = 1 ∑ N ( V n − μ ) 2 Standard Variance: σ \sigma σ Normal Distribution (All called Gaussian Distribution)f ( x ; μ , σ 2 ) = 1 2 π σ 2 e − ( x − μ ) 2 2 σ 2 f(x; \mu, \sigma^2) = \frac{1}{\sqrt{2\pi\sigma^2}}e^{\frac{-(x-\mu)^2}{2\sigma^2}} f ( x ; μ , σ 2 ) = 2 π σ 2 1 e 2 σ 2 − ( x − μ ) 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 valueInfo
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.
How to weight a x x x
Measure a gold Info
x x x z n z_n z n x ^ n , n \hat{x}_{n,n} x ^ n , n x x x x ^ n + 1 , n \hat{x}_{n+1,n} x ^ n + 1 , n n + 1 n+1 n + 1 n n n x n + 1 , n x_{n+1,n} x n + 1 , n x ^ n − 1 , n − 1 \hat{x}_{n−1,n−1} x ^ n − 1 , n − 1 x x x n − 1 n−1 n − 1 z n − 1 z_{n−1} z n − 1 n − 1 n−1 n − 1 x ^ n , n − 1 \hat{x}_{n,n-1} x ^ n , n − 1 n t h n^{th} n t h x n , n − 1 x_{n,n−1} x n , n − 1 x n + 1 , n x_{n+1, n} x n + 1 , n
In order to estimate x n , n x_{n,n} x n , n x n − 1 , n − 1 x_{n−1,n−1} x n − 1 , n − 1
Formula Meaning x ^ n , n = 1 n ∑ i = 1 n ( Z i ) \hat x_{n,n} = \frac{1}{n}\sum_{i=1}^{n}(Z_i) x ^ n , n = n 1 ∑ i = 1 n ( Z i ) The mean of measurement = 1 n ( ∑ i = 1 n − 1 ( Z i ) + Z n ) = \frac{1}{n}(\sum_{i=1}^{n-1}(Z_i)+ Z_n) = n 1 ( ∑ i = 1 n − 1 ( Z i ) + Z n ) = 1 n ∑ i = 1 n − 1 ( Z i ) + 1 n Z n = \frac{1}{n}\sum_{i=1}^{n-1}(Z_i)+ \frac{1}{n}Z_n = n 1 ∑ i = 1 n − 1 ( Z i ) + n 1 Z n = 1 n n − 1 n − 1 ∑ i = 1 n − 1 ( Z i ) + 1 n Z n = \frac{1}{n}\frac{n-1}{n-1}\sum_{i=1}^{n-1}(Z_i)+ \frac{1}{n}Z_n = n 1 n − 1 n − 1 ∑ i = 1 n − 1 ( Z i ) + n 1 Z n = n − 1 n 1 n − 1 ∑ i = 1 n − 1 ( Z i ) + 1 n Z n ) = \frac{n-1}{n}\textcolor{orange}{\frac{1}{n-1}\sum_{i=1}^{n-1}(Z_i)}+ \frac{1}{n}Z_n) = n n − 1 n − 1 1 ∑ i = 1 n − 1 ( Z i ) + n 1 Z n ) The orange item is the estimation at time n-1 = n − 1 n x ^ n − 1 , n − 1 + 1 n Z n ) = \frac{n-1}{n}\textcolor{orange}{\hat x_{n-1, n-1}}+ \frac{1}{n}Z_n) = n n − 1 x ^ n − 1 , n − 1 + n 1 Z n ) = x ^ n − 1 , n − 1 − 1 n x ^ n − 1 , n − 1 + 1 n Z n = \hat x_{n-1, n-1} - \frac{1}{n}\hat x_{n-1, n-1}+ \frac{1}{n}Z_n = x ^ n − 1 , n − 1 − n 1 x ^ n − 1 , n − 1 + n 1 Z n = x ^ n − 1 , n − 1 − 1 n ( x ^ n − 1 , n − 1 + Z n ) = \hat x_{n-1, n-1} - \frac{1}{n}(\hat x_{n-1, n-1} + Z_n) = x ^ n − 1 , n − 1 − n 1 ( 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.
v = Δ x Δ t v= \frac{\Delta x}{\Delta t} v = Δ t Δ x x n + 1 = x n + Δ t ∗ v n x_{n+1} = x_n + \Delta t*v_n x n + 1 = x n + Δ t ∗ v n v n + 1 = v n v_{n+1} = v_n v n + 1 = v n
Alt text Position: p t = p t − 1 + v t − 1 Δ t + u t Δ t 2 2 p_t = p_{t-1} + v_{t-1}\Delta t + u_t\frac{\Delta t^2}{2} p t = p t − 1 + v t − 1 Δ t + u t 2 Δ t 2 v t = v t − 1 + u t Δ t v_t = v_{t-1} + u_t\Delta t v t = v t − 1 + u t Δ t
The tow formula could be transfored into[ p t v t ] = [ 1 Δ t 0 1 ] [ p t − 1 v t − 1 ] + [ Δ t 2 2 Δ t ] u t \begin{bmatrix} p_t\\ v_t \end{bmatrix} = \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}\begin{bmatrix} p_{t-1}\\ v_{t-1} \end{bmatrix} + \begin{bmatrix} \frac{\Delta t^2}{2}\\ \Delta t \end{bmatrix}u_t [ p t v t ] = [ 1 0 Δ t 1 ] [ p t − 1 v t − 1 ] + [ 2 Δ t 2 Δ t ] u t
F t = [ 1 Δ t 0 1 ] , B t = [ Δ t 2 2 Δ t ] u t F_t = \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}, B_t = \begin{bmatrix} \frac{\Delta t^2}{2}\\ \Delta t \end{bmatrix}u_t F t = [ 1 0 Δ t 1 ] , B t = [ 2 Δ t 2 Δ t ] u t
Then the formulas could be transformed intox ^ − = F x ^ t − 1 + B u t − 1 \hat x^- = F\hat x_{t-1} + Bu_{t-1} x ^ − = F x ^ t − 1 + B u t − 1
If we want to track a plane, how to do it.x = x 0 + v 0 Δ t + 1 2 a Δ t 2 x = x_0 + v_0\Delta t + \frac{1}{2}a\Delta t^2 x = x 0 + v 0 Δ t + 2 1 a Δ t 2
Info
x x x x 0 x_0 x 0 v 0 v_0 v 0 a a a Δ t \Delta t Δ t
Because of we location a 3 dimentions world. There are 3 components in 3 different direction.
x = { x = x 0 + v x Δ t + 1 2 a x Δ t 2 y = y 0 + v y Δ t + 1 2 a y Δ t 2 z = z 0 + v z Δ t + 1 2 a z Δ t 2 x = \begin{cases} x= x_0 + v_x\Delta t + \frac{1}{2}a_x\Delta t^2 \\ y= y_0 + v_y\Delta t + \frac{1}{2}a_y\Delta t^2 \\ z= z_0 + v_z\Delta t + \frac{1}{2}a_z\Delta t^2 \end{cases} x = ⎩ ⎨ ⎧ x = x 0 + v x Δ t + 2 1 a x Δ t 2 y = y 0 + v y Δ t + 2 1 a y Δ t 2 z = z 0 + v z Δ t + 2 1 a z Δ t 2
So the state in three world would be like below.[ x y y v x v y v z a x a y a z ] \begin{bmatrix} x \\ y \\ y \\ v_x \\ v_y \\ v_z \\ a_x \\ a_y \\ a_z \end{bmatrix} x y y v x v y v z a x a y a z [ v x = x ˙ v y = y ˙ v z = z ˙ a x = x ¨ a y = y ¨ a z = z ¨ ] \begin{bmatrix} 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{bmatrix} v x = x ˙ v y = y ˙ v z = z ˙ a x = x ¨ a y = y ¨ a z = z ¨
{ 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 + 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 ) \begin{cases} Position_n &= Position_{(n-1)} + Velocity_{(n-1)}\Delta t + \frac{1}{2}Accelerate_{(n-1)}\Delta t^2 \\ Velocity_n &= Velocity_{(n-1)} + Accelerate_{(n-1)}\Delta t \\ Accelerate_n &= Accelerate_{(n+1)} \end{cases} ⎩ ⎨ ⎧ P os i t i o n n V e l oc i t y n A cce l er a t e n = P os i t i o n ( n − 1 ) + V e l oc i t y ( n − 1 ) Δ t + 2 1 A cce l er a t e ( n − 1 ) Δ t 2 = V e l oc i t y ( n − 1 ) + A cce l er a t e ( n − 1 ) Δ t = A cce l er a t e ( n + 1 )
[ x y y x ˙ y ˙ z ˙ x ¨ y ¨ z ¨ ] \begin{bmatrix} x \\ y \\ y \\ \dot{x} \\ \dot{y} \\ \dot{z} \\ \ddot{x} \\ \ddot{y} \\ \ddot{z} \end{bmatrix} x y y x ˙ y ˙ z ˙ x ¨ y ¨ z ¨
[ 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 ] \begin{bmatrix} position \\ velocity \\ acceleration \end{bmatrix} p os i t i o n v e l oc i t y a cce l er a t i o n
{ x n = x n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 y n = y n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 z n = z n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + 7 ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1 \begin{cases} x_n = x_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ y_n = y_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ z_n = z_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ \dot{x}_n = \dot{x}_{n-1} + \ddot{x}_{n-1}\Delta{t}\\ \dot{y}_n = \dot{y}_{n-1} + \ddot{7}_{n-1}\Delta{t}\\ \dot{z}_n = \dot{z}_{n-1} + \ddot{z}_{n-1}\Delta{t}\\ \ddot{x}_{n} = \ddot{x}_{n-1}\\ \ddot{y}_{n} = \ddot{y}_{n-1}\\ \ddot{z}_{n} = \ddot{z}_{n-1} \end{cases} ⎩ ⎨ ⎧ x n = x n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 y n = y n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 z n = z n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + 7 ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1
Predict State
Predict State: x ^ n + 1 , n = F x ^ n , n + G u n + ω n \hat{x}_{n+1,n} = F\hat{x}_{n,n} + Gu_n + \omega_n x ^ n + 1 , n = F x ^ n , n + G u n + ω n Predict Noise: P ^ n + 1 , n = F P ^ n , n F T + Q \hat{P}_{n+1,n} = F\hat{P}_{n,n}F^T + Q P ^ n + 1 , n = F P ^ n , n F T + Q
Update States
Kalman Gain: K n = P n , n − 1 H T ( H P n , n − 1 H T + R n ) − 1 K_n = P_{n, n-1}H^T(HP_{n,n-1}H^T + R_n)^{-1} K n = P n , n − 1 H T ( H P n , n − 1 H T + R n ) − 1 Update State: x ^ n , n = x ^ n , n − 1 + K n ( z n − H x ^ n , n − 1 ) \hat{x}_{n,n} = \hat{x}_{n,n-1} + K_n(z_n-H\hat{x}_{n,n-1}) x ^ n , n = x ^ n , n − 1 + K n ( z n − H 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 P_{n,n} = (I-K_nH)P_{n,n-1}(I-K_nH)^T + K_nR_nK_n^T P n , n = ( I − K n H ) P n , n − 1 ( I − K n H ) T + K n R n K n T
{ x n = x n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 y n = y n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 z n = z n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + 7 ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1 = [ 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 ] [ x ( n − 1 ) y ( n − 1 ) z ( n − 1 ) x ˙ ( n − 1 ) y ˙ ( n − 1 ) z ˙ ( n − 1 ) x ¨ ( n − 1 ) y ¨ ( n − 1 ) z ¨ ( n − 1 ) ] \begin{cases} x_n = x_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ y_n = y_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ z_n = z_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ \dot{x}_n = \dot{x}_{n-1} + \ddot{x}_{n-1}\Delta{t}\\ \dot{y}_n = \dot{y}_{n-1} + \ddot{7}_{n-1}\Delta{t}\\ \dot{z}_n = \dot{z}_{n-1} + \ddot{z}_{n-1}\Delta{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{bmatrix} 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2} & 0 & 0\\ 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2} & 0\\ 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2}\\ 0 & 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 &\Delta{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{bmatrix} \begin{bmatrix} 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{bmatrix} ⎩ ⎨ ⎧ x n = x n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 y n = y n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 z n = z n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + 7 ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1 = 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 x ( n − 1 ) y ( n − 1 ) z ( n − 1 ) x ˙ ( n − 1 ) y ˙ ( n − 1 ) z ˙ ( n − 1 ) x ¨ ( n − 1 ) y ¨ ( n − 1 ) z ¨ ( n − 1 )
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 u n u n = [ x ¨ n y ¨ n z ¨ n ] u_n = \begin{bmatrix} \ddot{x}_n\\ \ddot{y}_n\\ \ddot{z}_n \end{bmatrix} u n = x ¨ n y ¨ n z ¨ n
The state transition matrix F F F F = [ 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 ] F = \begin{bmatrix} 1 & 0 & 0 &\Delta{t} & 0 & 0\\ 0 & 1 & 0 & 0 &\Delta{t} & 0\\ 0 & 0 & 1 & 0 & 0 &\Delta{t}\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} F = 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1
The control matrix G G G G = [ 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 ] G = \begin{bmatrix} 0.5\Delta{t^2} & 0 & 0\\ 0 & 0.5\Delta{t^2} & 0\\ 0 & 0 & 0.5\Delta{t^2}\\ \Delta{t} & 0 & 0\\ 0 &\Delta{t} & 0\\ 0 & 0 &\Delta{t} \end{bmatrix} G = 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t
Thus the state prediction formula x ^ n + 1 , n = F x ^ n , n + G u n \hat{x}_{n+1,n} = F\hat{x}_{n,n} + Gu_n x ^ n + 1 , n = F x ^ n , n + G u n x ^ n + 1 , n = [ x ( n + 1 , n ) y ( n + 1 , n ) z ( n + 1 , n ) x ˙ ( n + 1 , n ) y ˙ ( n + 1 , n ) z ˙ ( n + 1 , n ) ] = [ 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 ] [ x ( n , n ) y ( n , n ) z ( n , n ) x ˙ ( n , n ) y ˙ ( n , n ) z ˙ ( n , n ) ] + [ 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 ] [ x ¨ n y ¨ n z ¨ n ] \hat{x}_{n+1,n} = \begin{bmatrix} 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{bmatrix} = \begin{bmatrix} 1 & 0 & 0 &\Delta{t} & 0 & 0\\ 0 & 1 & 0 & 0 &\Delta{t} & 0\\ 0 & 0 & 1 & 0 & 0 &\Delta{t}\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{(n,n)} \\ y_{(n,n)} \\ z_{(n,n)} \\ \dot{x}_{(n,n)} \\ \dot{y}_{(n,n)} \\ \dot{z}_{(n,n)} \end{bmatrix} + \begin{bmatrix} 0.5\Delta{t^2} & 0 & 0\\ 0 & 0.5\Delta{t^2} & 0\\ 0 & 0 & 0.5\Delta{t^2}\\ \Delta{t} & 0 & 0\\ 0 &\Delta{t} & 0\\ 0 & 0 &\Delta{t} \end{bmatrix}\begin{bmatrix} \ddot{x}_n\\ \ddot{y}_n\\ \ddot{z}_n \end{bmatrix} x ^ n + 1 , n = x ( n + 1 , n ) y ( n + 1 , n ) z ( n + 1 , n ) x ˙ ( n + 1 , n ) y ˙ ( n + 1 , n ) z ˙ ( n + 1 , n ) = 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1 x ( n , n ) y ( n , n ) z ( n , n ) x ˙ ( n , n ) y ˙ ( n , n ) z ˙ ( n , n ) + 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t x ¨ n y ¨ n z ¨ n
Consider an airplane moving in three-dimensional space with constant acceleration.
The state vector x ^ n = [ x y y x ˙ y ˙ z ˙ x ¨ y ¨ z ¨ ] \hat{x}_n = \begin{bmatrix} x \\ y \\ y \\ \dot{x} \\ \dot{y} \\ \dot{z} \\ \ddot{x} \\ \ddot{y} \\ \ddot{z} \end{bmatrix} x ^ n = x y y x ˙ y ˙ z ˙ x ¨ y ¨ z ¨
The state transition matrix F = [ 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 ] F = \begin{bmatrix} 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2} & 0 & 0\\ 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2} & 0\\ 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2}\\ 0 & 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 &\Delta{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{bmatrix} F = 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1
The state extrapolation equation is:[ 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 ] [ x ( n − 1 ) y ( n − 1 ) z ( n − 1 ) x ˙ ( n − 1 ) y ˙ ( n − 1 ) z ˙ ( n − 1 ) x ¨ ( n − 1 ) y ¨ ( n − 1 ) z ¨ ( n − 1 ) ] → { x n = x n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 y n = y n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 z n = z n − 1 + v n − 1 Δ t + 1 2 a n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + 7 ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1 \begin{bmatrix} 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2} & 0 & 0\\ 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2} & 0\\ 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0 & 0.5\Delta{t^2}\\ 0 & 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 &\Delta{t} & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 &\Delta{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{bmatrix} \begin{bmatrix} 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{bmatrix} \rightarrow \begin{cases} x_n = x_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ y_n = y_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ z_n = z_{n-1} + v_{n-1}\Delta t + \frac{1}{2}a_{n-1}\Delta t^2\\ \dot{x}_n = \dot{x}_{n-1} + \ddot{x}_{n-1}\Delta{t}\\ \dot{y}_n = \dot{y}_{n-1} + \ddot{7}_{n-1}\Delta{t}\\ \dot{z}_n = \dot{z}_{n-1} + \ddot{z}_{n-1}\Delta{t}\\ \ddot{x}_{n} = \ddot{x}_{n-1}\\ \ddot{y}_{n} = \ddot{y}_{n-1}\\ \ddot{z}_{n} = \ddot{z}_{n-1} \end{cases} 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 1 x ( n − 1 ) y ( n − 1 ) z ( n − 1 ) x ˙ ( n − 1 ) y ˙ ( n − 1 ) z ˙ ( n − 1 ) x ¨ ( n − 1 ) y ¨ ( n − 1 ) z ¨ ( n − 1 ) → ⎩ ⎨ ⎧ x n = x n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 y n = y n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 z n = z n − 1 + v n − 1 Δ t + 2 1 a n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + 7 ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1
he state vector x ^ n \hat{x}_n x ^ n
x ^ n = { x ^ n y ^ n z ^ n x ˙ ^ n y ˙ ^ n z ˙ ^ n \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} x ^ n = ⎩ ⎨ ⎧ x ^ n y ^ n z ^ n x ˙ ^ n y ˙ ^ n z ˙ ^ n
The control vector u n u_n u n describes the measured airplane acceleration in a cartesian coordinate systemu n = [ x ¨ n y ¨ n z ¨ n ] u_n = \begin{bmatrix} \ddot{x}_n\\ \ddot{y}_n\\ \ddot{z}_n \end{bmatrix} u n = x ¨ n y ¨ n z ¨ n
The state transition matrix: F = [ 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 ] F = \begin{bmatrix} 1 & 0 & 0 &\Delta{t} & 0 & 0\\ 0 & 1 & 0 & 0 &\Delta{t} & 0\\ 0 & 0 & 1 & 0 & 0 &\Delta{t}\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} F = 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1
The control matrix: G = [ 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 ] G = \begin{bmatrix} 0.5\Delta{t^2} & 0 & 0\\ 0 & 0.5\Delta{t^2} & 0\\ 0 & 0 & 0.5\Delta{t^2}\\ \Delta{t} & 0 & 0\\ 0 &\Delta{t} & 0\\ 0 & 0 &\Delta{t} \end{bmatrix} G = 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t
Thus the state prediction formula x ^ n + 1 , n = F x ^ n , n + G u n \hat{x}_{n+1,n} = F\hat{x}_{n,n} + Gu_n x ^ n + 1 , n = F x ^ n , n + G u n
x ^ n + 1 , n = [ x ( n + 1 , n ) y ( n + 1 , n ) z ( n + 1 , n ) x ˙ ( n + 1 , n ) y ˙ ( n + 1 , n ) z ˙ ( n + 1 , n ) ] = [ 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 ] [ x ( n , n ) y ( n , n ) z ( n , n ) x ˙ ( n , n ) y ˙ ( n , n ) z ˙ ( n , n ) ] + [ 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 ] [ x ¨ n y ¨ n z ¨ n ] \hat{x}_{n+1,n} = \begin{bmatrix} 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{bmatrix} = \begin{bmatrix} 1 & 0 & 0 &\Delta{t} & 0 & 0\\ 0 & 1 & 0 & 0 &\Delta{t} & 0\\ 0 & 0 & 1 & 0 & 0 &\Delta{t}\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{(n,n)} \\ y_{(n,n)} \\ z_{(n,n)} \\ \dot{x}_{(n,n)} \\ \dot{y}_{(n,n)} \\ \dot{z}_{(n,n)} \end{bmatrix} + \begin{bmatrix} 0.5\Delta{t^2} & 0 & 0\\ 0 & 0.5\Delta{t^2} & 0\\ 0 & 0 & 0.5\Delta{t^2}\\ \Delta{t} & 0 & 0\\ 0 &\Delta{t} & 0\\ 0 & 0 &\Delta{t} \end{bmatrix}\begin{bmatrix} \ddot{x}_n\\ \ddot{y}_n\\ \ddot{z}_n \end{bmatrix} x ^ n + 1 , n = x ( n + 1 , n ) y ( n + 1 , n ) z ( n + 1 , n ) x ˙ ( n + 1 , n ) y ˙ ( n + 1 , n ) z ˙ ( n + 1 , n ) = 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1 0 0 0 Δ t 0 0 1 x ( n , n ) y ( n , n ) z ( n , n ) x ˙ ( n , n ) y ˙ ( n , n ) z ˙ ( n , n ) + 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t 0 0 0 0.5Δ t 2 0 0 Δ t x ¨ n y ¨ n z ¨ n
Kalman Filter 卡尔曼滤波器 Kalman filter [Math]理解卡尔曼滤波器 (Understanding Kalman Filter) 卡尔曼滤波器的原理以及在matlab中的实现 
Project - Kalman Filter (Discard)
