Board Reference

    Flight Controller
    Field Oriented Controller

    Motor Drive

    PMSM

    Motion Control

    Attitude Control

    Tutorial

    Mathematics
    Classical Control
    State Estimation
    Feedforward Control
    Disturbance Observer
    Motion Control

Kalman filter

A Kalman filter estimates a state from a viewpoint of statistics. let us consider a system
x[k+1]=Ax[k]+Bu[k]+Gvy[k]=Cx[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}
where x\bm{x}, y\bm{y}, and u\bm{u} are a state, output, and input; A\bm{A}, B\bm{B}, and C\bm{C} stand for a system matrix, input matrix, and observation matrix; v\bm{v} and w\bm{w} are a process noise and observation noise that satisfy
E[vvTwvTvwTwwT]=[Q00R].\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}
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:

Prediction step

xp[k+1]=AmxK[k]+Bmu[k]yp[k]=Cmxp[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}
where a subscription m_{\rm m} stands for a model parameter, and subscriptions p_{\rm p} and K_{\rm K} denote a predicted value and filter output.

Filtering step

xK[k]=xp[k]+K[k]ey[k]ey[k]y[k]yp,\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}
where K\bm{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[ey].\begin{align} J={\rm E}[|\bm{e}_{\rm \bm{y}}|]. \end{align}
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]xp[k+1]=Ax[k]AmxK[k]+Bu[k]Bmu[k]+Gv=A(x[k]xp[k])+Gvep[k+1]=AeK[k]+Gv.(ep[k]x[k]xp[k], eK[k]x[k]xK[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}
Then, an error covariance of prediction state is calculated as
ep[k+1]epT[k+1]=(AeK[k]+Gv)(AeK[k]+Gv)T=(AeK[k]+Gv)(eKT[k]AT+vTGT)=AeK[k]eKT[k]AT+AeK[k]vTGT+GveKT[k]AT+GvvTGTPp[k+1]=APK[k]AT+GQGT.(Pp[k]cov[ep[k]], PK[k]cov[eK[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}

Transition of estimation error covariance

An estimated error is calculated as
eK[k]=x[k]xK[k]=x[k](xp[k]+K[k]ey[k])=x[k]xp[k]K[k]y[k]+K[k]Cmxp[k]=x[k]xp[k]K[k]Cx[k]+K[k]Cmxp[k]+K[k]w=(IK[k]C)ep[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}
Then, an error covariance of a filtered state is calculated as
eK[k]eKT[k]=((IK[k]C)ep[k]+K[k]w)((IK[k]C)ep[k]+K[k]w)T=((IK[k]C)ep[k]+K[k]w)(epT[k](IK[k]C)T+wTKT[k])=(IK[k]C)ep[k]epT[k](IK[k]C)T+K[k]wwTKT[k]+K[k]wepT[k](IK[k]C)T+(IK[k]C)ep[k]wTKT[k]PK[k]=(IK[k]C)Pp[k](IK[k]C)T+K[k]RKT[k]=Pp[k]K[k]CPp[k]Pp[k]CTKT[k]+K[k](CPp[k]CT+R)KT[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}

Kalman gain

To minimize the cost function, the filtering system should satisfies a following condition
tr(PK[k])K[k]=2(CPp[k])T+2K[k](CPp[k]CT+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}
This condition derives
K[k]=PpT[k]CT(CPp[k]CT+R)1=Pp[k]CT(CPp[k]CT+R)1.(Pp[k]=PpT[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}
Assuming that a Kalman gain is appropriately set, the prediction process of an error covariance PK\bm{P}_{\rm K} can be simplified as
PK[k]=Pp[k]K[k]CPp[k]Pp[k]CTKT[k]+K[k](CPp[k]CT+R)KT[k]=Pp[k]K[k]CPp[k]Pp[k]CTKT[k]+Pp[k]CTKT[k]=(IK[k]C)Pp[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}

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 Pp\bm{P}_{\rm p} can be expanded as
Pp[k+1]=APK[k]AT+GQGT=A(IK[k]C)Pp[k]AT+GQGT=A(IPp[k]CT(CPp[k]CT+R)1C)Pp[k]AT+GQGT=A(Pp[k]Pp[k]CT(CPp[k]CT+R)1CPp[k])AT+GQGT.\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}
In a steady state, an error covariance is invariant and a following equation holds.
Pp=A(PpPpCT(CPpCT+R)1CPp)AT+GQGT.\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}
Here, a Kalman gain also get a constant value, and a observer is called a steady-state Kalman filter.
K=Pp[k]CT(CPp[k]CT+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}

Filtering characteristics

An output is composed of a low-pass filtered state and predicted state.
xK[k]=xp[k]+K[k](y[k]Cmxp[k])=(IK[k]Cm)xp[k]+K[k]Cx[k]+K[k]w=(IK[k]C)xp[k]+K[k](CCm)xp+K[k]Cx[k]+K[k]w=(IF[k])(AmxK[k1]+Bmu[k1])+F[k](Ax[k1]+Bu[k1]+Gv)+K[k]w+Δ=(IF[k])AmxK[k1]+F[k](Ax[k1]+Gv)+Bmu[k1]+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}
where
F[k]K[k]C=Pp[k]CT(CPp[k]CT+R)1CΔK[k](CCm)xp. \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}
The filter gain is determined by the norms of process/observation noises. F[k]\bm{F}[k] indicates an internal division point of these norms.
ey[k]=y[k]yp[k]=Cx[k]Cmxp[k]+w=Cep[k]+wey[k]eyT[k]=(Cep[k]+w)(Cep[k]+w)T=(Cep[k]+w)(epT[k]CT+wT)=Cep[k]epT[k]CT+Cep[k]w+wepT[k]CT+wwTPy[k]=CPp[k]CT+RF[k]=K[k]C=Pp[k]CTPy1[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}

Simplified process

A predicted state xp\bm{x}_{\rm p} is an output of a Luenberger observer and a filtered state xK\bm{x}_{\rm K} is a smoothed signal of xp\bm{x}_{\rm p}.
xp[k+1]=AmxK[k]+Bmu[k]xK[k]=xp[k]+K[k](y[k]Cmxp[k])xp[k+1]=Amxp[k]+Bmu[k]+AmK[k](y[k]Cmxp[k])xK[k]=(IF[k])xp[k]+Fx[k]+K[k](CCm)xp+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}

© DigitalServo