A Kalman filter estimates a state from a viewpoint of statistics.
let us consider a system
x [ k + 1 ] = A x [ k ] + B u [ k ] + G v y [ k ] = C x [ k ] + w , \begin{align}
\bm{x}[k+1]&=\bm{A}\bm{x}[k]+\bm{B}\bm{u}[k]+\bm{G}\bm{v} \\
\bm{\bm{y}}[k]&=\bm{C}\bm{x}[k]+\bm{w},
\end{align} x [ k + 1 ] y [ k ] = A x [ k ] + B u [ k ] + G v = C x [ k ] + w ,
where
x \bm{x} x ,
y \bm{y} y , and
u \bm{u} u are a state, output, and input;
A \bm{A} A ,
B \bm{B} B , and
C \bm{C} C stand for a system matrix, input matrix, and observation matrix;
v \bm{v} v and
w \bm{w} w are a process noise and observation noise that satisfy
E [ v v T w v T v w T w w T ] = [ Q 0 0 R ] . \begin{align}
{\rm E}
\begin{bmatrix}
\bm{v}\bm{v}^{\mathrm T} & \bm{w}\bm{v}^{\mathrm T} \\
\bm{v}\bm{w}^{\mathrm T} & \bm{w}\bm{w}^{\mathrm T}
\end{bmatrix} =
\begin{bmatrix}
\bm{Q} & \bm{0} \\
\bm{0} & \bm{R}
\end{bmatrix}.
\end{align} E [ v v T v w T w v T w w T ] = [ Q 0 0 R ] .
The filtering characteristic of a Kalman filter is determined by the variances of a process noise and observation noise.
A filtering process is composed of two steps:
x p [ k + 1 ] = A m x K [ k ] + B m u [ k ] y p [ k ] = C m x p [ k ] , \begin{align}
\bm{x}_{\rm p}[k+1]&=\bm{A}_{\rm m}\bm{x}_{\rm K}[k]+\bm{B}_{\rm m}\bm{u}[k] \\
\bm{y}_{\rm p}[k]&=\bm{C}_{\rm m}\bm{x}_{\rm p}[k],
\end{align} x p [ k + 1 ] y p [ k ] = A m x K [ k ] + B m u [ k ] = C m x p [ k ] ,
where a subscription
m _{\rm m} m stands for a model parameter,
and subscriptions
p _{\rm p} p and
K _{\rm K} K denote a predicted value and filter output.
x K [ k ] = x p [ k ] + K [ k ] e y [ k ] e y [ k ] ≡ y [ k ] − y p , \begin{align}
\bm{x}_{\rm K}[k]&=\bm{x}_{\rm p}[k]+\bm{K}[k]\bm{e}_{\rm \bm{y}}[k] \\
\bm{e}_{\rm \bm{y}}[k] &\equiv \bm{\bm{y}}[k]-\bm{y}_{\rm p},
\end{align} x K [ k ] e y [ k ] = x p [ k ] + K [ k ] e y [ k ] ≡ y [ k ] − y p ,
where
K \bm{K} K is a Kalman gain.
This gain is uniquely sought by system information.
Derivation of Kalman gain
A Kalmain gain is selected to minimize a cost function
J = E [ ∣ e y ∣ ] . \begin{align}
J={\rm E}[|\bm{e}_{\rm \bm{y}}|].
\end{align} J = E [ ∣ e y ∣ ] .
Such a gain is uniquely determined by focusing on the transition of an estimation error covariance.
Transition of prediction error covariance
A prediction error is calculated as
x [ k + 1 ] − x p [ k + 1 ] = A x [ k ] − A m x K [ k ] + B u [ k ] − B m u [ k ] + G v = A ( x [ k ] − x p [ k ] ) + G v ∴ e p [ k + 1 ] = A e K [ k ] + G v . ( e p [ k ] ≡ x [ k ] − x p [ k ] , e K [ k ] ≡ x [ k ] − x K [ k ] ) \begin{align}
\bm{x}[k+1]-\bm{x}_{\rm p}[k+1]&=\bm{A}\bm{x}[k]-\bm{A}_{\rm m}\bm{x}_{\rm K}[k]+\bm{B}\bm{u}[k]-\bm{B}_{\rm m}\bm{u}[k] +\bm{G}\bm{v} \\
&= \bm{A}(\bm{x}[k]-\bm{x}_{\rm p}[k])+\bm{G}\bm{v} \\
\therefore \bm{e}_{\rm p}[k+1]&=\bm{A}\bm{e}_{\rm K}[k]+\bm{G}\bm{v}. \\
(\bm{e}_{\rm p}[k]&\equiv \bm{x}[k]-\bm{x}_{\rm p}[k],\ \bm{e}_{\rm K}[k]\equiv \bm{x}[k]-\bm{x}_{\rm K}[k])
\end{align} x [ k + 1 ] − x p [ k + 1 ] ∴ e p [ k + 1 ] ( e p [ k ] = A x [ k ] − A m x K [ k ] + B u [ k ] − B m u [ k ] + G v = A ( x [ k ] − x p [ k ]) + G v = A e K [ k ] + G v . ≡ x [ k ] − x p [ k ] , e K [ k ] ≡ x [ k ] − x K [ k ])
Then, an error covariance of prediction state is calculated as
e p [ k + 1 ] e p T [ k + 1 ] = ( A e K [ k ] + G v ) ( A e K [ k ] + G v ) T = ( A e K [ k ] + G v ) ( e K T [ k ] A T + v T G T ) = A e K [ k ] e K T [ k ] A T + A e K [ k ] v T G T + G v e K T [ k ] A T + G v v T G T ∴ P p [ k + 1 ] = A P K [ k ] A T + G Q G T . ( P p [ k ] ≡ c o v [ e p [ k ] ] , P K [ k ] ≡ c o v [ e K [ k ] ] ) \begin{align}
\bm{e}_{\rm p}[k+1]\bm{e}_{\rm p}^{\mathrm T}[k+1]&=(\bm{A}\bm{e}_{\rm K}[k]+\bm{G}\bm{v})(\bm{A}\bm{e}_{\rm K}[k]+\bm{G}\bm{v})^{\mathrm T} \\
&=(\bm{A}\bm{e}_{\rm K}[k]+\bm{G}\bm{v})(\bm{e}_{\rm K}^{\rm T}[k]\bm{A}^{\mathrm T}+\bm{v}^{\mathrm T}\bm{G}^{\mathrm T}) \\
&= \bm{A}\bm{e}_{\rm K}[k]\bm{e}_{\rm K}^{\mathrm T}[k]\bm{A}^{\mathrm T}+\bm{A}\bm{e}_{\rm K}[k]\bm{v}^{\mathrm T}\bm{G}^{\mathrm T}+\bm{G}\bm{v}\bm{e}_{\rm K}^{\rm T}[k]\bm{A}^{\rm T}+\bm{G}\bm{v}\bm{v}^{\mathrm T}\bm{G}^{\mathrm T} \\
\therefore \bm{P}_{\rm p}[k+1]&=\bm{A}\bm{P}_{\rm K}[k]\bm{A}^{\mathrm T}+\bm{G}\bm{Q}\bm{G}^{\mathrm T}. \\
(\bm{P}_{\rm p}[k]&\equiv {\rm cov}\left[\bm{e}_{\rm p}[k]\right],\ \bm{P}_{\rm K}[k]\equiv{\rm cov}\left[\bm{e}_{\rm K}[k]\right])
\end{align} e p [ k + 1 ] e p T [ k + 1 ] ∴ P p [ k + 1 ] ( P p [ k ] = ( A e K [ k ] + G v ) ( A e K [ k ] + G v ) T = ( A e K [ k ] + G v ) ( e K T [ k ] A T + v T G T ) = A e K [ k ] e K T [ k ] A T + A e K [ k ] v T G T + G v e K T [ k ] A T + G v v T G T = A P K [ k ] A T + G Q G T . ≡ cov [ e p [ k ] ] , P K [ k ] ≡ cov [ e K [ k ] ] )
Transition of estimation error covariance
An estimated error is calculated as
e K [ k ] = x [ k ] − x K [ k ] = x [ k ] − ( x p [ k ] + K [ k ] e y [ k ] ) = x [ k ] − x p [ k ] − K [ k ] y [ k ] + K [ k ] C m x p [ k ] = x [ k ] − x p [ k ] − K [ k ] C x [ k ] + K [ k ] C m x p [ k ] + K [ k ] w = ( I − K [ k ] C ) e p [ k ] + K [ k ] w \begin{align} \bm{e}_{\rm K}[k]&=\bm{x}[k]-\bm{x}_{\rm K}[k]\\&=\bm{x}[k]-(\bm{x}_{\rm p}[k]+\bm{K}[k]\bm{e}_{\rm \bm{y}}[k]) \\
&= \bm{x}[k]-\bm{x}_{\rm p}[k]-\bm{K}[k]\bm{\bm{y}}[k]+\bm{K}[k]\bm{C}_{\rm m}\bm{x}_{\rm p}[k] \\
&= \bm{x}[k]-\bm{x}_{\rm p}[k]-\bm{K}[k]\bm{C}\bm{x}[k]+\bm{K}[k]\bm{C}_{\rm m}\bm{x}_{\rm p}[k]+\bm{K}[k]\bm{w} \\
&= (\bm{I}-\bm{K}[k]\bm{C})\bm{e}_{\rm p}[k]+\bm{K}[k]\bm{w} \end{align} e K [ k ] = x [ k ] − x K [ k ] = x [ k ] − ( x p [ k ] + K [ k ] e y [ k ]) = x [ k ] − x p [ k ] − K [ k ] y [ k ] + K [ k ] C m x p [ k ] = x [ k ] − x p [ k ] − K [ k ] C x [ k ] + K [ k ] C m x p [ k ] + K [ k ] w = ( I − K [ k ] C ) e p [ k ] + K [ k ] w
Then, an error covariance of a filtered state is calculated as
e K [ k ] e K T [ k ] = ( ( I − K [ k ] C ) e p [ k ] + K [ k ] w ) ( ( I − K [ k ] C ) e p [ k ] + K [ k ] w ) T = ( ( I − K [ k ] C ) e p [ k ] + K [ k ] w ) ( e p T [ k ] ( I − K [ k ] C ) T + w T K T [ k ] ) = ( I − K [ k ] C ) e p [ k ] e p T [ k ] ( I − K [ k ] C ) T + K [ k ] w w T K T [ k ] + K [ k ] w e p T [ k ] ( I − K [ k ] C ) T + ( I − K [ k ] C ) e p [ k ] w T K T [ k ] ∴ P K [ k ] = ( I − K [ k ] C ) P p [ k ] ( I − K [ k ] C ) T + K [ k ] R K T [ k ] = P p [ k ] − K [ k ] C P p [ k ] − P p [ k ] C T K T [ k ] + K [ k ] ( C P p [ k ] C T + R ) K T [ k ] . \begin{align}
\bm{e}_{\rm K}[k]\bm{e}_{\rm K}^{\mathrm T}[k]=&\left((\bm{I}-\bm{K}[k]\bm{C})\bm{e}_{\rm p}[k]+\bm{K}[k]\bm{w}\right)\left((\bm{I}-\bm{K}[k]\bm{C})\bm{e}_{\rm p}[k]+\bm{K}[k]\bm{w}\right)^{\mathrm T} \\
=&\left((\bm{I}-\bm{K}[k]\bm{C})\bm{e}_{\rm p}[k]+\bm{K}[k]\bm{w}\right)\left(\bm{e}_{\rm p}^{\mathrm T}[k](\bm{I}-\bm{K}[k]\bm{C})^{\mathrm T}+\bm{w}^{\mathrm T}\bm{K}^{\mathrm T}[k]\right) \\
=&(\bm{I}-\bm{K}[k]\bm{C})\bm{e}_{\rm p}[k]\bm{e}_{\rm p}^{\mathrm T}[k](\bm{I}-\bm{K}[k]\bm{C})^{\mathrm T}+\bm{K}[k]\bm{w}\bm{w}^{\mathrm T}\bm{K}^{\mathrm T}[k]\\
&+\bm{K}[k]\bm{w} \bm{e}_{\rm p}^{\mathrm T}[k](\bm{I}-\bm{K}[k]\bm{C})^{\mathrm T}+(\bm{I}-\bm{K}[k]\bm{C})\bm{e}_{\rm p}[k]\bm{w}^{\mathrm T}\bm{K}^{\mathrm T}[k] \\
\therefore \bm{P}_{\rm K}[k]=& (\bm{I}-\bm{K}[k]\bm{C})\bm{P}_{\rm p}[k](\bm{I}-\bm{K}[k]\bm{C})^{\mathrm T}+\bm{K}[k]\bm{R}\bm{K}^{\mathrm T}[k] \\
=& \bm{P}_{\rm p}[k]- \bm{K}[k]\bm{C}\bm{P}_{\rm p}[k]-\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}\bm{K}^{\mathrm T}[k]+\bm{K}[k](\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})\bm{K}^{\mathrm T}[k].
\end{align} e K [ k ] e K T [ k ] = = = ∴ P K [ k ] = = ( ( I − K [ k ] C ) e p [ k ] + K [ k ] w ) ( ( I − K [ k ] C ) e p [ k ] + K [ k ] w ) T ( ( I − K [ k ] C ) e p [ k ] + K [ k ] w ) ( e p T [ k ] ( I − K [ k ] C ) T + w T K T [ k ] ) ( I − K [ k ] C ) e p [ k ] e p T [ k ] ( I − K [ k ] C ) T + K [ k ] w w T K T [ k ] + K [ k ] w e p T [ k ] ( I − K [ k ] C ) T + ( I − K [ k ] C ) e p [ k ] w T K T [ k ] ( I − K [ k ] C ) P p [ k ] ( I − K [ k ] C ) T + K [ k ] R K T [ k ] P p [ k ] − K [ k ] C P p [ k ] − P p [ k ] C T K T [ k ] + K [ k ] ( C P p [ k ] C T + R ) K T [ k ] .
To minimize the cost function, the filtering system should satisfies a following condition
∂ t r ( P K [ k ] ) ∂ K [ k ] = − 2 ( C P p [ k ] ) T + 2 K [ k ] ( C P p [ k ] C T + R ) = 0. \begin{align}
\frac{\partial {\rm tr}(\bm{P}_{\rm K }[k])}{\partial \bm{K}[k]} &= -2(\bm{C}\bm{P}_{\rm p}[k])^{\mathrm T}+2\bm{K}[k](\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})=0.
\end{align} ∂ K [ k ] ∂ tr ( P K [ k ]) = − 2 ( C P p [ k ] ) T + 2 K [ k ] ( C P p [ k ] C T + R ) = 0.
This condition derives
K [ k ] = P p T [ k ] C T ( C P p [ k ] C T + R ) − 1 = P p [ k ] C T ( C P p [ k ] C T + R ) − 1 . ( ∵ P p [ k ] = P p T [ k ] ) \begin{align}
\bm{K}[k] &= \bm{P}_{\rm p}^{\mathrm T}[k] \bm{C}^{\mathrm T}(\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})^{-1} \\
&= \bm{P}_{\rm p}[k] \bm{C}^{\mathrm T}(\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})^{-1}. \\
&(\because \bm{P}_{\rm p}[k]=\bm{P}_{\rm p}^{\mathrm T}[k])
\end{align} K [ k ] = P p T [ k ] C T ( C P p [ k ] C T + R ) − 1 = P p [ k ] C T ( C P p [ k ] C T + R ) − 1 . ( ∵ P p [ k ] = P p T [ k ])
Assuming that a Kalman gain is appropriately set, the prediction process of an error covariance
P K \bm{P}_{\rm K} P K can be simplified as
P K [ k ] = P p [ k ] − K [ k ] C P p [ k ] − P p [ k ] C T K T [ k ] + K [ k ] ( C P p [ k ] C T + R ) K T [ k ] = P p [ k ] − K [ k ] C P p [ k ] − P p [ k ] C T K T [ k ] + P p [ k ] C T K T [ k ] = ( I − K [ k ] C ) P p [ k ] . \begin{align}
\bm{P}_{\rm K}[k]&= \bm{P}_{\rm p}[k]- \bm{K}[k]\bm{C}\bm{P}_{\rm p}[k]-\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}\bm{K}^{\mathrm T}[k]+\bm{K}[k](\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})\bm{K}^{\mathrm T}[k] \\
&= \bm{P}_{\rm p}[k]- \bm{K}[k]\bm{C}\bm{P}_{\rm p}[k]-\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}\bm{K}^{\mathrm T}[k]+\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}\bm{K}^{\mathrm T}[k] \\
&= (\bm{I}-\bm{K}[k]\bm{C})\bm{P}_{\rm p}[k].
\end{align} P K [ k ] = P p [ k ] − K [ k ] C P p [ k ] − P p [ k ] C T K T [ k ] + K [ k ] ( C P p [ k ] C T + R ) K T [ k ] = P p [ k ] − K [ k ] C P p [ k ] − P p [ k ] C T K T [ k ] + P p [ k ] C T K T [ k ] = ( I − K [ k ] C ) P p [ k ] .
Steady-state Kalman filter
A Kalmain filter for a linear time-invariant system has a constant error covariance matrix in a steady-state.
The prediction process of error covariance
P p \bm{P}_{\rm p} P p can be expanded as
P p [ k + 1 ] = A P K [ k ] A T + G Q G T = A ( I − K [ k ] C ) P p [ k ] A T + G Q G T = A ( I − P p [ k ] C T ( C P p [ k ] C T + R ) − 1 C ) P p [ k ] A T + G Q G T = A ( P p [ k ] − P p [ k ] C T ( C P p [ k ] C T + R ) − 1 C P p [ k ] ) A T + G Q G T . \begin{align}
\bm{P}_{\rm p}[k+1]&=\bm{A}\bm{P}_{\rm K}[k]\bm{A}^{\mathrm T}+\bm{G}\bm{Q}\bm{G}^{\mathrm T} \\
&=\bm{A}(\bm{I}-\bm{K}[k]\bm{C})\bm{P}_{\rm p}[k]\bm{A}^{\mathrm T}+\bm{G}\bm{Q}\bm{G}^{\mathrm T} \\
&=\bm{A}(\bm{I}-\bm{P}_{\rm p}[k] \bm{C}^{\mathrm T}(\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})^{-1}\bm{C})\bm{P}_{\rm p}[k]\bm{A}^{\mathrm T}+\bm{G}\bm{Q}\bm{G}^{\mathrm T} \\
&=\bm{A}(\bm{P}_{\rm p}[k]-\bm{P}_{\rm p}[k] \bm{C}^{\mathrm T}(\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})^{-1}\bm{C}\bm{P}_{\rm p}[k])\bm{A}^{\mathrm T}+\bm{G}\bm{Q}\bm{G}^{\mathrm T}.
\end{align} P p [ k + 1 ] = A P K [ k ] A T + G Q G T = A ( I − K [ k ] C ) P p [ k ] A T + G Q G T = A ( I − P p [ k ] C T ( C P p [ k ] C T + R ) − 1 C ) P p [ k ] A T + G Q G T = A ( P p [ k ] − P p [ k ] C T ( C P p [ k ] C T + R ) − 1 C P p [ k ]) A T + G Q G T .
In a steady state, an error covariance is invariant and a following equation holds.
P p ∗ = A ( P p ∗ − P p ∗ C T ( C P p ∗ C T + R ) − 1 C P p ∗ ) A T + G Q G T . \begin{align}
\bm{P}^{\ast}_{\rm p}=\bm{A}(\bm{P}^{\ast}_{\rm p}-\bm{P}^{\ast}_{\rm p} \bm{C}^{\mathrm T}(\bm{C}\bm{P}^{\ast}_{\rm p}\bm{C}^{\mathrm T}+\bm{R})^{-1}\bm{C}\bm{P}^{\ast}_{\rm p})\bm{A}^{\mathrm T}+\bm{G}\bm{Q}\bm{G}^{\mathrm T}.
\end{align} P p ∗ = A ( P p ∗ − P p ∗ C T ( C P p ∗ C T + R ) − 1 C P p ∗ ) A T + G Q G T .
Here, a Kalman gain also get a constant value, and a observer is called a steady-state Kalman filter.
K ∗ = P p ∗ [ k ] C T ( C P p ∗ [ k ] C T + R ) − 1 . \begin{align}
\bm{K}^{\ast} &= \bm{P}^{\ast}_{\rm p}[k] \bm{C}^{\mathrm T}(\bm{C}\bm{P}^{\ast}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})^{-1}.
\end{align} K ∗ = P p ∗ [ k ] C T ( C P p ∗ [ k ] C T + R ) − 1 .
Filtering characteristics
An output is composed of a low-pass filtered state and predicted state.
x K [ k ] = x p [ k ] + K [ k ] ( y [ k ] − C m x p [ k ] ) = ( I − K [ k ] C m ) x p [ k ] + K [ k ] C x [ k ] + K [ k ] w = ( I − K [ k ] C ) x p [ k ] + K [ k ] ( C − C m ) x p + K [ k ] C x [ k ] + K [ k ] w = ( I − F [ k ] ) ( A m x K [ k − 1 ] + B m u [ k − 1 ] ) + F [ k ] ( A x [ k − 1 ] + B u [ k − 1 ] + G v ) + K [ k ] w + Δ = ( I − F [ k ] ) A m x K [ k − 1 ] + F [ k ] ( A x [ k − 1 ] + G v ) + B m u [ k − 1 ] + K [ k ] w + Δ , \begin{align}
\bm{x}_{\rm K}[k]&=\bm{x}_{\rm p}[k]+\bm{K}[k](\bm{\bm{y}}[k]-\bm{C}_{\rm m}\bm{x}_{\rm p}[k]) \\
&=(\bm{I}-\bm{K}[k]\bm{C}_{\rm m})\bm{x}_{\rm p}[k]+\bm{K}[k]\bm{C}\bm{x}[k]+\bm{K}[k]\bm{w} \\
&=(\bm{I}-\bm{K}[k]\bm{C})\bm{x}_{\rm p}[k]+ \bm{K}[k](\bm{C}-\bm{C}_{\rm m})\bm{x}_{\rm p} + \bm{K}[k]\bm{C}\bm{x}[k]+\bm{K}[k]\bm{w} \\
&=(\bm{I}-\bm{F}[k])(\bm{A}_{\rm m}\bm{x}_{\rm K}[k-1]+\bm{B}_{\rm m}\bm{u}[k-1])+\bm{F}[k](\bm{A}\bm{x}[k-1]+\bm{B}\bm{u}[k-1]+\bm{G}\bm{v})+\bm{K}[k]\bm{w} + \Delta \\
&=(\bm{I}-\bm{F}[k])\bm{A}_{\rm m}\bm{x}_{\rm K}[k-1]+\bm{F}[k](\bm{A}\bm{x}[k-1]+\bm{G}\bm{v})+\bm{B}_{\rm m}\bm{u}[k-1] +\bm{K}[k]\bm{w} +\Delta, \\
\end{align} x K [ k ] = x p [ k ] + K [ k ] ( y [ k ] − C m x p [ k ]) = ( I − K [ k ] C m ) x p [ k ] + K [ k ] C x [ k ] + K [ k ] w = ( I − K [ k ] C ) x p [ k ] + K [ k ] ( C − C m ) x p + K [ k ] C x [ k ] + K [ k ] w = ( I − F [ k ]) ( A m x K [ k − 1 ] + B m u [ k − 1 ]) + F [ k ] ( A x [ k − 1 ] + B u [ k − 1 ] + G v ) + K [ k ] w + Δ = ( I − F [ k ]) A m x K [ k − 1 ] + F [ k ] ( A x [ k − 1 ] + G v ) + B m u [ k − 1 ] + K [ k ] w + Δ ,
where
F [ k ] ≡ K [ k ] C = P p [ k ] C T ( C P p [ k ] C T + R ) − 1 C Δ ≡ K [ k ] ( C − C m ) x p . \begin{align}
\bm{F}[k] & \equiv \bm{K}[k]\bm{C} = \bm{P}_{\rm p}[k] \bm{C}^{\mathrm T}(\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R})^{-1}\bm{C}\\
\Delta & \equiv \bm{K}[k](\bm{C}-\bm{C}_{\rm m})\bm{x}_{\rm p}.
\end{align} F [ k ] Δ ≡ K [ k ] C = P p [ k ] C T ( C P p [ k ] C T + R ) − 1 C ≡ K [ k ] ( C − C m ) x p .
The filter gain is determined by the norms of process/observation noises.
F [ k ] \bm{F}[k] F [ k ] indicates an internal division point of these norms.
e y [ k ] = y [ k ] − y p [ k ] = C x [ k ] − C m x p [ k ] + w = C e p [ k ] + w ⇔ e y [ k ] e y T [ k ] = ( C e p [ k ] + w ) ( C e p [ k ] + w ) T = ( C e p [ k ] + w ) ( e p T [ k ] C T + w T ) = C e p [ k ] e p T [ k ] C T + C e p [ k ] w + w e p T [ k ] C T + w w T ∴ P y [ k ] = C P p [ k ] C T + R F [ k ] = K [ k ] C = P p [ k ] C T P y − 1 [ k ] C . \begin{align}
\bm{e}_{\rm \bm{y}}[k] &=\bm{\bm{y}}[k]-\bm{y}_{\rm p}[k] \\
&=\bm{C}\bm{x}[k]-\bm{C}_{\rm m}\bm{x}_{\rm p}[k] +\bm{w} \\
&=\bm{C}\bm{e}_{\rm p}[k]+\bm{w}\\
\Leftrightarrow \bm{e}_{\rm \bm{y}}[k]\bm{e}_{\rm \bm{y}}^{\mathrm T}[k]&= (\bm{C}\bm{e}_{\rm p}[k]+\bm{w})(\bm{C}\bm{e}_{\rm p}[k]+\bm{w})^{\mathrm T} \\
&= (\bm{C}\bm{e}_{\rm p}[k]+\bm{w})(\bm{e}_{\rm p}^{\mathrm T}[k]\bm{C}^{\mathrm T}+\bm{w}^{\mathrm T}) \\
&= \bm{C}\bm{e}_{\rm p}[k]\bm{e}_{\rm p}^{\mathrm T}[k]\bm{C}^{\mathrm T}+\bm{C}\bm{e}_{\rm p}[k]\bm{w}+\bm{w}\bm{e}_{\rm p}^{\mathrm T}[k]\bm{C}^{\mathrm T}+\bm{w}\bm{w}^{\mathrm T} \\
\therefore \bm{P}_{\rm \bm{y}}[k]&=\bm{C}\bm{P}_{\rm p}[k]\bm{C}^{\mathrm T}+\bm{R}\\
\bm{F}[k] &= \bm{K}[k]\bm{C} = \bm{P}_{\rm p}[k] \bm{C}^{\mathrm T}\bm{P}_{\rm \bm{y}}^{-1}[k]\bm{C}.
\end{align} e y [ k ] ⇔ e y [ k ] e y T [ k ] ∴ P y [ k ] F [ k ] = y [ k ] − y p [ k ] = C x [ k ] − C m x p [ k ] + w = C e p [ k ] + w = ( C e p [ k ] + w ) ( C e p [ k ] + w ) T = ( C e p [ k ] + w ) ( e p T [ k ] C T + w T ) = C e p [ k ] e p T [ k ] C T + C e p [ k ] w + w e p T [ k ] C T + w w T = C P p [ k ] C T + R = K [ k ] C = P p [ k ] C T P y − 1 [ k ] C .
A predicted state
x p \bm{x}_{\rm p} x p is an output of a Luenberger observer and a filtered state
x K \bm{x}_{\rm K} x K is a smoothed signal of
x p \bm{x}_{\rm p} x p .
x p [ k + 1 ] = A m x K [ k ] + B m u [ k ] x K [ k ] = x p [ k ] + K [ k ] ( y [ k ] − C m x p [ k ] ) ⇔ x p [ k + 1 ] = A m x p [ k ] + B m u [ k ] + A m K [ k ] ( y [ k ] − C m x p [ k ] ) x K [ k ] = ( I − F [ k ] ) x p [ k ] + F x [ k ] + K [ k ] ( C − C m ) x p + K [ k ] w . \begin{align}
\bm{x}_{\rm p}[k+1]&=\bm{A}_{\rm m}\bm{x}_{\rm K}[k]+\bm{B}_{\rm m}\bm{u}[k] \\
\bm{x}_{\rm K}[k]&=\bm{x}_{\rm p}[k]+\bm{K}[k](\bm{\bm{y}}[k]-\bm{C}_{\rm m}\bm{x}_{\rm p}[k]) \\
\Leftrightarrow \bm{x}_{\rm p}[k+1]&=\bm{A}_{\rm m}\bm{x}_{\rm p}[k]+\bm{B}_{\rm m}\bm{u}[k]+\bm{A}_{\rm m}\bm{K}[k](\bm{\bm{y}}[k]-\bm{C}_{\rm m}\bm{x}_{\rm p}[k])\\
\bm{x}_{\rm K}[k]&=(\bm{I}-\bm{F}[k])\bm{x}_{\rm p}[k] + \bm{F}\bm{x}[k] + \bm{K}[k](\bm{C}-\bm{C}_{\rm m})\bm{x}_{\rm p} + \bm{K}[k]\bm{w}.
\end{align} x p [ k + 1 ] x K [ k ] ⇔ x p [ k + 1 ] x K [ k ] = A m x K [ k ] + B m u [ k ] = x p [ k ] + K [ k ] ( y [ k ] − C m x p [ k ]) = A m x p [ k ] + B m u [ k ] + A m K [ k ] ( y [ k ] − C m x p [ k ]) = ( I − F [ k ]) x p [ k ] + F x [ k ] + K [ k ] ( C − C m ) x p + K [ k ] w .