[PYTHON] Non-linear Kalman filter (1)
About this series
Since I studied the Kalman filter for non-linear phenomena in a book, I will summarize it as a memo. The Kalman filter is famous as an algorithm for sequentially optimizing and estimating the state vector of a linear probability system based on observation data. The Kalman filter assumes linearity in the time evolution of the state vector and the observation of the state vector. Therefore, some ingenuity is required to apply it to non-linear phenomena. In this series, we will proceed from the linear Kalman filter to the implementation of the nonlinear Kalman filter. The reference material is [Non-linear Kalman Filter (Toru Katayama)](https://www.amazon.co.jp/%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%82%AB % E3% 83% AB% E3% 83% 9E% E3% 83% B3% E3% 83% 95% E3% 82% A3% E3% 83% AB% E3% 82% BF-% E7% 89% 87% E5% B1% B1-% E5% BE% B9 / dp / 4254201486).
This time, let us consider the case where both state transitions and observations are linear (can be described by matrix operations). TL;DR The code (Jupyter notebook) is here https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb
Linear probability system represented by a state space model
\begin{align}
x_{t+1} = F_t x_t + G_t w_t \\
y_t = H_tx_t + v_t\\
\end{align}
Think about. Where $ x_t \ in \ mathbb {R} ^ n $ is the state vector, $ y_t \ in \ mathbb {R} ^ p $ is the observation vector, $ w_t \ in \ mathbb {R} ^ m $, $ v_t \ in \ mathbb {R} ^ p $ is a Gaussian noise vector, and $ F_t \ in \ mathbb {R} ^ {n \ times n} $ is a transition matrix, $ G_t \ in \ mathbb {R} ^ {n \ times m} $ is called the driving matrix, and $ H_t \ in \ mathbb {R} ^ {p \ times n} $ is called the observation matrix. In addition, we introduce $ P, Q, R $ as error covariance matrices for $ x_t $, $ w_t $, $ v_t $ respectively. Hurry up, you can make a Kalman filter by implementing the following formulas. The subscript $ t / t-1 $ represents the operation (prediction) of estimating the state of time $ t $ using the information of time $ t-1 $, and $ t / t $ uses the information of the current time. Represents the operation for optimizing the state (filtering). $ T / N $ represents the operation for optimizing the time $ t $ (smoothing) when the prediction is completed up to the time $ N $.
Kalman filter
- Set the initial value to $ \ hat {x} \ _ {0 / -1} = \ bar {x} _0 $, $ P \ _ {0 / -1} = P \ _0 $, $ t = 0 Let's say $.
- Observation update step
a. Kalman gain
- Time update step
a. One step ahead predicted value
Kalman smoother
\begin{align}
\hat{x}_{t/N} &= \hat{x}_{t/t} + C_t(\hat{x}_{t+1/N} - \hat{x}_{t+1/t})\\
C_t &= P_{t/t} F_t^T P_{t+1/t}^{-1} \\
P_{t/N} &= P_{t/t} + C_t [P_{t+1/N}-P_{t+1/t}]C_t^T\\
\end{align}
Implementation
Sequential prediction of a dynamic linear system is performed using a Kalman filter. As shown in the example on p.56 of the reference material, consider a four-dimensional system that linearly approximates the rotational motion of an artificial satellite.
\begin{align}
\dot{x}_t^{(1)} &= x_t^{(2)}\\
\dot{x}_t^{(2)} &= x_t^{(3)} + x_t^{(4)}\\
\dot{x}_t^{(3)} &= 0\\
\dot{x}_t^{(1)} &= -0.5 x_t^{(4)} + \xi_t \\
\end{align}
However, $ {x} \ _ t ^ {(1)} $: Attitude angle of artificial satellite, $ {x} \ _ t ^ {(2)} $: Angular velocity, $ {x} \ _ t ^ {(3)} $: Mean component of angular acceleration, $ {x} \ _ t ^ {(4)} $: Random component of angular acceleration, $ \ xi_t $ is Gaussian noise with mean 0. Sample interval $ \ Delta = 1.0 When discrete with $, the time evolution is described below.
x_{t+1} = \left(
\begin{array}{ccc}
1 & 1 & 0.5 & 0.5\\
0 & 1 & 1 & 1\\
0 & 0 & 1 & 0\\
0 & 0 & 0 & 0.606
\end{array}
\right) x_t + \left(
\begin{array}{ccc}
0\\
0\\
0\\
1
\end{array}
\right) w_t
However, the variance of Gaussian noise $ w_t and v_t $ is $ Q = 0.64 \ times 10 ^ {-2} $, $ R = 1 $. The initial value of the state vector and the initial value of the estimated value
x_0 = \left(\begin{array}{ccc}
1.25\\
0.06\\
0.01\\
-0.003
\end{array}
\right),
\hat{x}_{0/-1} = \left(\begin{array}{ccc}
0\\
0\\
0\\
0
\end{array}
\right),
P_{0/-1} = \rm{diag}[10,10,10,10]
The simulation is performed as. The result of satellite attitude angle estimation is shown in the figure below.
The filter estimates on the green line are greatly influenced by the observed values, but the smoothing estimates on the red line appear to be approaching the true movement of the attitude angle.
The code (Jupyter notebook) is here https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb
A little more detailed story
Coming soon