“This is the fifth day of my participation in the First Challenge 2022. For details: First Challenge 2022”

State observer

In engineering, it is necessary to introduce state observer when some state parameters are too expensive to be measured or cannot be measured with existing instruments. The state observer takes the input and output of the original system as inputs and outputs the estimated state variables.

Description of original system status:


{ x k = A x k – 1 + B u k – 1 y k = C x k \begin{cases} x_k=Ax_{k-1}+Bu_{k-1}\\ y_k=Cx_k\\\end{cases}

State observer state description:


{ x ~ k = A x ~ k – 1 + B u k – 1 + f [ y k – 1 – y ~ k – 1 ] y ~ k = C x ~ k \begin{cases} \tilde{x}_k=A\tilde{x}_{k-1}+Bu_{k-1}+f\left[ y_{k-1}-\tilde{y}_{k-1} \right]\\ \tilde{y}_k=C\tilde{x}_k\\\end{cases}

If the error vector EK = Xk −x~ ke_K = X_K-\ tilde{x}_kek= xK − X ~ K, the equations of state of the two systems can be subtracted


System error transfer equation e k = ( A – f C ) e k – 1 System Error transfer equation e_k=\left(a-fc \right) e_{k-1}

The general solution is matrix exponential function, so when the eigenvalue of A− FCA-FCA-FC is less than 0, each component of the error vector eKE_KEk tends to 0, that is, the state observer can directly estimate the original system state.

Essentially, a state observer is a mathematical module that converges from an incorrect state estimate to a correct state estimate for a deterministic system described by a state space equation.

2 state filter

The actual system is often a random process, and its states are described as follows:


{ x k = A x k – 1 + B u k – 1 + w k – 1 y k = C x k + v k \begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ y_k=Cx_k+v_k\\\end{cases}

Among them, WKW_KWK is called process noise, which is mainly caused by external forces in the process of system operation, such as gust of wind received by rotorcraft in the process of movement. Vkv_kvk is called measurement noise, which is mainly caused by measuring instrument error and insufficient accuracy in the process of system measurement.

In essence, a state filter is a mathematical model for optimal estimation of system state by extracting information from uncertain observations for a random system described by a random state space equation. When WKW_KWK and VKVK are gaussian white noise and independent of each other, the state filter is kalman filter.

The core of the state filter is to adjust the gain matrix of the filter continuously through the Bayes principle to reduce the random interference and make the estimated system state approach the real state. The core of state observer is to determine observer gain matrix through pole assignment, so that the estimated state can track the current state of the system.

Generally, the performance of the state filter is better than that of the state observer, but in the actual system, if the random noise is non-Gaussian noise, or there are several nonlinear links, the state filter established according to the conventional noise model may diverge, so the stability of the state observer is more needed.

Kalman filter

Machine learning: Bayesian network + Sample Analysis


Parameter prior information PI. ( Theta. ) + Sample observation data X = Posterior distribution P ( Theta. ∣ X ) \ text {} parameters a priori information, PI, left (\ theta \ right) + \ text} {sample observation data X = \ text} {the posterior distribution P \ left (\ theta \ | X right)

In other words, prior distribution π(θ)\ PI \left(\theta \right)π(θ) is the cognition of the model before the new sample information is obtained, and after the new sample information XXX is obtained, People’s perception of the model for the posterior distribution P (theta ∣ X) P \ left (\ theta \ | X right) P (theta ∣ X).

This leads to kalman filter. Consider a random system:


{ x k = A x k – 1 + B u k – 1 + w k – 1 z k = C x k + v k \begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k=Cx_k+v_k\\\end{cases}

Where wk N(0,Q)w_k~N\left(0,Q \right)wk N(0,Q), vk N(0,R)v_k~N\left(0,R \right)vk N(0,R), and wkw_KWk and VKv_kvk are independent.

A prior mathematical model of the system can be established by ignoring noise (noise only satisfies random distribution and cannot be modeled) :


State prediction equation { x ^ k – = A x ^ k – 1 + B u k – 1 z ^ k – = C x ^ k {\ text condition forecasting equation} {\ begin {cases} \ hat {x} _ {k} ^ {-} = A \ hat {x} _ {1} k + Bu_} {k – 1 \ \ \ hat {z} _ {k} ^ {-} = C \ hat {x} _k \ \ \ end {cases}}

Based on the above system, the state update equation of Kalman filter is derived from the Bayesian method:


State update equation : x ^ k = x ^ k – + K k ( z k – z ^ k – ) {\ text} {status update equation: \ hat {x} _k = \ hat {x} _ {k} ^ {} + K_k \ left (z_k – \ hat {z} _ {k} ^ {to} \ right)}

Where X ^k\hat{x}_kx^k is a posterior state estimate, that is, the optimal estimate of the current system state after a correction; X ^k− hat{x}_{k}^{-}x^k− is a prior estimate of the system state; Zkz_kzk is the measurement sample value of the current state; Z ^k− hat{z}_{k}^{-}z^k− is the prior measurement of the current state; Zk – z ^ k – z_k – \ hat {z} _ {k} ^ ^ k {and} zk – z – represent the actual system and estimate the influence of the random error of the system; The relationship between KkK_kKk balance prior model and measured data for posterior distribution is called Kalman gain. Obviously, the smaller zK − Z ^ K − Z_K -\hat{z}_{K}^{-}zk−z^ K − is, the closer the actual and predicted are, and the closer the posterior distribution is to the prior distribution. The larger zk−z^k− Z_K -\hat{z}_{k}^{-}zk−z^ K − is, the less reliable the prediction is.

The next step is to make sure that KkK_kKk is the best modification effect of the posterior model, that is, the closer x^k\hat{x} _Kx ^k is to the true value xk{x}_kxk. Define error vector


{ A posterior error vector e k = x k – x ^ k Prior error vector e k – = x k – x ^ k – \ begin {cases} \ text {} a posteriori error vector e_k = x_k – \ hat {x} _k \ \ \ text {} a priori error vector e_ ^ {k} {to} = x_k – \ hat {x} _ {k} ^ {cases} {to} \ \ \ end

The posterior error covariance matrix is defined as:


P k = E ( e k e k T ) = [ sigma e 1 2 sigma e 1 sigma e 2 … sigma e 1 sigma e n sigma e 2 sigma e 1 sigma e 2 2 … sigma e 2 sigma e n ⋮ ⋮ ⋱ ⋮ sigma e n sigma e 1 sigma e n sigma e 2 … sigma e n 2 ] P_k=E\left( e_ke_{k}^{T} \right) =\left[ \begin{matrix} \sigma _{e1}^{2}& \sigma _{e1}\sigma _{e2}& \cdots& \sigma _{e1}\sigma _{en}\\ \sigma _{e2}\sigma _{e1}& \sigma _{e2}^{2}& \cdots& \sigma _{e2}\sigma _{en}\\ \vdots& \vdots& \ddots& \vdots\\ \sigma _{en}\sigma _{e1}& \sigma _{en}\sigma _{e2}& \cdots& \sigma _{en}^{2}\\\end{matrix} \right]

Similarly, there is a prior error covariance matrix Pk−P_{k}^{-}Pk−. According to the principle of least variance estimation, let the loss function be tr(Pk)\mathrm{tr}\left(P_k \right)tr(Pk), i.e


K k = a r g min ⁡ [ t r ( P k ) ] K_k=\mathrm{arg}\min \left[ \mathrm{tr}\left( P_k \right) \right]

will
P k P_k For:The state update equation and state prediction equation are used above, considering that
e k – e_{k}^{-} with
v k v_k Independent of each other, further:


P k = ( I – K k C ) E ( e k – e k – T ) ( I – K k C ) T + K k E ( v k v k T ) K k T = ( I – K k C ) P k – ( I – K k C ) T + K k R K k T P_k=\left( I-K_kC \right) E\left( e_{k}^{-}{e_{k}^{-}}^T \right) \left( I-K_kC \right) ^T+K_kE\left( v_kv_{k}^{T} \right) K_{k}^{T}\\=\left( I-K_kC \right) P_{k}^{-}\left( I-K_kC \right) ^T+K_kRK_{k}^{T}

Make partial tr (Pk) partial Kk = 0 \ frac {\ partial \ mathrm {tr} \ left (P_k \ right)} {\ partial K_k} = 0 partial Kk partial tr (Pk) = 0, namely


Kalman gain adjustment equation K k = P k – C T C P k – C T + R {\ text} {kalman gain adjustment equation K_k = \ frac {P_ {k} ^ ^ T} {} – C {CP_ ^ {k} {} – C ^ T + R}}

Substitute KkK_kKk into the posterior error covariance matrix expression, get


Covariance update equation P k = ( I – K k H ) P k – {\ text} {covariance updating equation P_k = \ left (I – K_kH \ right) P_ {k} ^ {and}}

To update KkK_kKk, only the prior error covariance matrix Pk−P_{K}^{-}Pk− is determined. Likewise, Pk−P_{k}^{-}Pk− is expanded as


P k – = E ( e k – e k – T ) = E [ ( x k – x ^ k – ) ( x k – x ^ k – ) T ] = E [ ( A e k – 1 + w k – 1 ) ( A e k – 1 + w k – 1 ) T ] = A E ( e k – 1 e k – 1 T ) A T + E ( w k – 1 w k – 1 T ) P_{k}^{-}=E\left( e_{k}^{-}e_{k}^{-T} \right) \\=E\left[ \left( x_k-\hat{x}_{k}^{-} \right) \left( x_k-\hat{x}_{k}^{-} \right) ^T \right] \\=E\left[ \left( Ae_{k-1}+w_{k-1} \right) \left( Ae_{k-1}+w_{k-1} \right) ^T \right] \\=AE\left( e_{k-1}e_{k-1}^{T} \right) A^T+E\left( w_{k-1}w_{k-1}^{T} \right)

Considering that eKE_KEk and WKW_kwk are independent from each other, further obtain


Covariance prediction equation P k – = A P k – 1 A T + Q {\text{covariance prediction equation}P_{k}^{-}=AP_{k-1}A^T+Q}

So far, five basic formulas of Kalman filter have been obtained, among which the core equation is kalman gain adjustment equation based on minimum variance estimation. The specific work flow is shown in the figure.

4 Specific case: Ship GPS positioning

A ship left the port and sailed along a straight line to assist the Beidou satellite in positioning and speed measurement. Assuming ① Ship acceleration a(t)a\left(t \right)a(t)= maneuvering acceleration u(t)u\left(t \right)u(t)+ random acceleration w(t)w\left(t \right)w(t), Where w(t)w\left(t \right)w(t) conforms to gaussian distribution; ② GPS observation noise v(t)v\left(t \right)v(t) conforms to Gaussian distribution. Kalman filter is required to estimate the real trajectory.

The solution

First, build a real model of the stochastic system. Taking the starting point of the wharf as the origin, sampling period (radar scanning period) as TTT, s(k) S (k) S (k) S (k) is used to represent the real position of the ship at sampling time kTkTkT, and Z (k)z\left(k \right)z(k) is used to represent the GPS positioning observation value at sampling time kTkTkT. The real system output equation for z (k) = s (k) + v (k) z (k) (k) = s + v (k) (k) z = s + v (k) (k).

Computation of ship speed S ˙(k)\dot{s}\left(k \right)s˙(k), so the acceleration is a(k)a\left(k \right)a(k). According to the uniform acceleration formula,


s ( k + 1 ) = s ( k ) + s ˙ ( k ) T + 1 2 a ( k ) T 2 ⇒ s ˙ ( k + 1 ) = s ˙ ( k ) + T a ( k ) \left(k+1 \right) =s\left(k \right) +ṡ\left(k \right) T+\frac{1}{2}a\left(k \right) T^2\ dot{s}\left(k \right) =s\left(k \right) +ṡ\left(k \right) T+\frac{1}{2}a\left(k \right) T^2\ dot{s}\left( K +1 =ṡ\left(k \right) +Ta\left(k \right)

With a (k) = u (k) + w (k) a (k) = u (k) + w (k), a (k) = u (k) + w (k).

The system state x(k)x\left(k \right)x(k) at the sampling time kTkTkT is defined as the position and speed of the ship, The x (k) = [s (k) s ˙ (k)] Tx (k) = \ left [\ begin {matrix} s (k) & \ dot {s} (k)] {matrix} \ \ \ \ end right ^ Tx (k) = [s (k) s ˙ (k)] T of the state space model of ship motion can be obtained


{ [ s ( k + 1 ) s ˙ ( k + 1 ) ] = [ 1 T 0 1 ] [ s ( k ) s ˙ ( k ) ] + [ 0.5 T T ] u ( k ) + [ 0.5 T T ] w ( k ) z ( k ) = [ 1 0 ] [ s ( k ) s ˙ ( k ) ] + v ( k ) \begin{cases} \left[ \begin{array}{c} s(k+1)\\ \dot{s}(k+1)\\\end{array} \right] =\left[ \begin{matrix} 1& T\\ 0& 1 \ \ \ end {matrix} \ \ left right] [\ begin {array} {c} s (k) \ \ \ dot {s} (k) {array} \ \ \ \ end right] + \ left [\ begin {array} {c} 0.5 T \ \ T {array} \ \ \ \ end u (k) + \ left right] [\ begin {array} {c} 0.5 T \ \ T \] {array} \ \ \ end right w (k) (k) = \ \ \ z left [\ begin 1 & {matrix} 0\\\end{matrix} \right] \left[ \begin{array}{c} s(k)\\ \dot{s}(k)\\\end{array} \right] +v(k)\\\end{cases}

When the dynamic factor of the maneuvering target is not considered, namely U (k)=0u(k)=0u(k)=0, the ship system with uniform and linear motion is extended to four dimensions, and the state includes horizontal and longitudinal position and velocity, then the system equation can be written into


[ x ( k ) x ˙ ( k ) y ( k ) y ˙ ( k ) ] = [ 1 T 0 0 0 1 0 0 0 0 1 T 0 0 0 1 ] [ x ( k – 1 ) x ˙ ( k – 1 ) y ( k – 1 ) y ˙ ( k – 1 ) ] + [ 0.5 T 2 T 0.5 T 2 T ] [ w 1 ( k ) w 2 ( k ) w 3 ( k ) w 4 ( k ) ] z ( k ) = [ 1 0 0 0 0 0 1 0 ] [ x ( k ) x ˙ ( k ) y ( k ) y ˙ ( k ) ] + [ v 1 ( k ) v 2 ( k ) ] \left[ \begin{array}{c} x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\\\end{array} \right] =\left[ \begin{matrix} 1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\\\end{matrix} \right] \left[ \begin{array}{c} x(k-1)\\ \dot{x}(k-1)\\ y(k-1)\\ \ dot {} y (k – 1) {array} \ \ \ \ end right] + \ left [\ begin {matrix} 0.5 T ^ 2 & & & & & \ \ \ \ & T & T ^ 2 & 0.5 T \ \ \ \ \ & & & end {matrix} \right] \left[ \begin{array}{c} w_1\left( k \right)\\ w_2\left( k \right)\\ w_3\left( k \right)\\ w_4\left( k \right)\\\end{array} \right] \\z(k)=\left[ \begin{matrix} 1& 0& 0& 0\\ 0& 0& 1& 0\\\end{matrix} \right] \left[ \begin{array}{c} x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\\\end{array} \right] +\left[ \begin{array}{c} v_1\left( k \right)\\ v_2\left( k \right)\\\end{array} \right]

It is assumed that the initial position is (-100m, 200m), the initial velocity of horizontal movement is 2m/s, the initial velocity of vertical movement is 20m/s, and the radar scanning period T=1sT=1sT=1s, then the state space of the system is further reduced to


{ x ( k + 1 ) = A x ( k ) + B u ( k ) + Γ w ( k ) z ( k ) = H x ( k ) + v ( k ) \begin{cases} x(k+1)=Ax(k)+Bu(k)+\varGamma w(k)\\ z(k)=Hx(k)+v(k)\\\end{cases}

A=[1100010000110001]A=\left[\begin{matrix} 1&1&0&0 \\ 0&1&0&0 \\ 0&0&1 \\ 0&0&1 \\end{matrix} \ A = right] ⎣ ⎢ ⎢ ⎢ ⎡ 1000110000100011 ⎦ ⎥ ⎥ ⎥ ⎤, B = Γ = = B \ varGamma = \ [0.510.51] left [\ begin {matrix} 0.5 & & & 1 && \ \ \ \ & & & & 0.5 \ \ & & & 1 \ \ \ end {matrix} \ B = right] Γ = ⎣ ⎢ ⎢ ⎢ ⎡ 0.510.51 ⎦ ⎥ ⎥ ⎥ ⎤, H=[10000010]H=\left[\begin{matrix} 1&0&0&0\0&0&1&0 \\end{matrix} \right]H=[10000100].

Suppose the process noise w ~ N(0,0.12)w\sim N(0,0.1^2)w ~ N(0,0.12) and its four components are independently and identically distributed, then


Q ∗ = E ( w 4 x 1 w 4 x 1 T ) = sigma w 2 [ 1 1 1 1 ] Q^*=E(w_{4\times 1}w_{4\times 1}^{T})=\sigma _{w}^{2}\left[ \begin{matrix} 1& & & \\ & 1& & \\ & & 1& \\ & & & 1\\\end{matrix} \right]

Considering the weighting of γ \varGamma γ, the process noise covariance matrix is obtained


Q = Q ∗ ⋅ Γ = sigma w 2 [ 0.5 1 0.5 1 ] Q = Q \ cdot \ varGamma = \ sigma ^ * _ ^ {w} {2} \ left [\ begin {matrix} 0.5 & & & 1 && \ \ \ \ & & & & 0.5 \ \ & & & 1 \ \ \ end {matrix} \right]

Observe noise v ~ N(0,102)v\sim N(0,10^2)v ~ N(0,102), its two components are independently and identically distributed, then observe noise covariance matrix


R = sigma v 2 [ 1 1 ] R=\sigma _{v}^{2}\left[ \begin{matrix} 1& \\ & 1\\\end{matrix} \right]

Based on this, the kalman filter model is established.

Setting the optimal estimate initial x ^ (1) = \ [- 1002-20020] T hat {x} (1) = \ left [\ begin {matrix} – 100 & 2 & – 200 & 20 {matrix} \ \ \ \ end right] ^ Tx ^ (1) = 1002-20020 – T, a priori covariance matrix P1 – = [1000010000100001] P_ {1} ^ {to} = \ left [\ begin {matrix} 1 & & & 0 0 0 \ \ 0 & & & 0 \ \ 0 0 1 & & 0 and 1 0\\ 0&0&0&1 \\\end{matrix} \right]P1−=⎣⎢⎢⎢, 1000010000100001 x ⎥⎥⎥⎤.

When using kalman filter, at least one of the system model or system measurement should have enough accuracy, otherwise kalman filter cannot extract correct estimation information from it. This is because kalman filter adjusts the trust weight between the prior model value and the instrumental measurement value. If both are inaccurate, the kalman tracking value is also inaccurate. To verify this conclusion, the measurement noise is kept unchanged and the process noise variance is adjusted, as shown in the figure.

Complete code private message acquisition.