kalman滤波应用

1 无噪牛顿系统

假设一个牛顿系统，包含位置\(r\)，速度\(v\)和加速度\(a\). 这个系统可以描述为：

\begin{equation} \label{eq:1} \begin{bmatrix} \overset{\cdot}{r} \\ \overset{\cdot}{v} \\ \overset{\cdot}{a} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} r \\ v \\ a \end{bmatrix} \end{equation} \begin{equation} \label{eq:3} \overset{\cdot}{x} = A x \end{equation}

这是连续系统的描述方式，离散化之后可以表示为：

\begin{equation} \label{eq:2} x_{k+1} = F x_{k} \end{equation}

其中\(F = \exp(AT)\) ，展开有：

\begin{eqnarray*} F&=&\exp(AT) \\ &=& I + AT + \frac{(AT)^{2}}{2!} + \ldots \\ &=& \begin{bmatrix} 1 & T & \frac{(AT)^{2}}{2!} \\ 0 & 1 & T \\ 0 & 0 & 0 \end{bmatrix} \end{eqnarray*}

其中\(T\)是离散化的采样时间。

这个系统的kalman滤波描述为：

\begin{eqnarray*} \hat{x}_{k}^{-}&=&F\hat{x}_{k-1}^{+} \\7 P_{k}^{-} &=& FP_{k-1}^{+}F^{T} \end{eqnarray*}

我们发现在\(( k-1 )^{+}\)到\(k^{-}\)之间，估计误差的协方差是增加的。因为在\((k-1)^{+}\)和\(k^{-}\)之间我们没有收到任何观察值，估计误差增大也是可以理解的。

现在假定我们对位置进行测量，测量方差为\(\sigma^{2}\):

\begin{equation} \label{eq:4} y_{k} = H_{k}x_{k} + v_{k} \end{equation}

其中

\begin{eqnarray*} v_{k}&\sim & N(0,R_{k}) \\ R_{k} &=& \sigma^{2} \end{eqnarray*}

我们可以计算出Kalman增益：

\begin{equation} \label{eq:5} K_{k} = P_{k}^{-}H_{k}^{T} (H_{k}P_{k}^{-}H_{k}^{T} + R_{k})^{-1} \end{equation}

把\(K_{k}\)，\(H_{k}\)，\(R_{k}\)带入，有：

\begin{equation} \label{eq:6} K_{k} = \begin{bmatrix} P_{k,11}^{-} \\ P_{k,12}^{-} \\ P_{k,13}^{-} \end{bmatrix} \frac{1}{P_{k,11}^{-} + \sigma^2} \end{equation}

估计误差协方差：

\begin{equation} \label{eq:7} P_{k}^{+} = P_{k}^{-} - K_{k}H_{k}P_{k}^{-} \end{equation}

如果我们把\(P_{k}^{-}\) 完整的写出来，并替换\(H_{k},K_{k}\)，可得：

\begin{eqnarray} \label{eq:8} P_{k}^{+}&=& P_{k}^{-} - \frac{1}{P_{k,11}^{-} + \sigma^{2}} \begin{bmatrix} P_{k,11}^{-} & 0 & 0 \\ P_{k,12}^{-} & 0 & 0 \\ P_{k,13}^{-} & 0 & 0 \end{bmatrix} P_k^- \\ &=& P_k^- - \frac{1}{P_{k,11}^{-} + \sigma^2} \begin{bmatrix} (P_{k,11}^{-})^{2} & P_{k,11}^{-}P_{k,21}^{-} & P_{k,11}^{-}P_{k,31}^{-} \\ P_{k,12}^{-}P_{k,11}^{-} & (P_{k,12}^{-})^{2} & P_{k,12}^{-}P_{k,31}^{-} \\ P_{k,13}^{-}P_{k,11}^{-} & P_{k,13}^{-}P_{k,12}^{-} & (P_{k,13}^{-})^{2} \end{bmatrix} \end{eqnarray}

通过式~(\ref{eq:8})，我们将看到从\(k^{-}\)到\(k^{+}\)，协方差矩阵的迹下降。当我们得到一个新的观测时，我们希望对系统的状态估计会有所改进。也就是说，我们希望协方差降低。式~(\ref{eq:8})告诉我们这个协方差的确降低了。

2 matlab实现

在使用matlab实现之前，让我们再次总结一下离散时间kalman滤波器的过程。首先，这个系统的描述为：

\begin{eqnarray*} x_{k}&=&F_{k-1}x_{k-1} + G_{k-1}u_{k-1} + w_{k-1} \\ y_{k}&=& H_{k}x_{k} + v_{k} \\ E(w_{k}w_{j}^{T}) &=& Q_{k}\delta_{k-j} \\ E(v_{k}v_{j}^{T}) &=& R_{k}\delta_{k-j} \\ E(w_{k}v_{j}^{T}) &=& 0 \end{eqnarray*}

Kalman滤波器初始化为：

\begin{eqnarray} \label{eq:9} \hat{x}_{0}^{+}&=&E(x_{0}) \\ P_{0}^{+} &=& E[(x_{0} - \hat{x}_{0}^{+})(x_{0} - \hat{x}_{0}^{+})^{T}] \end{eqnarray}

Kalman滤波器的整个过程为：

\begin{eqnarray*} P_{k}^{-}&=&F_{k-1}P_{k-1}^{+}F_{k-1}^{T} + Q_{k-1} \\ K_{k} &=& P_{k}^{-}H_{k}^{T}(H_{k}P_{k}^{-}H_{k}^{T} + R_{k})^{-1} \\ &=& P_{k}^{+}H_{k}^{T}R_{k}^{-1} \\ \hat{x}_{k}^{-} &=& F_{k-1} \hat{x}_{k-1}^{+} + G_{k-1}u_{k-1} \\ \hat{x}_{k}^{+} &=& \hat{x}_{k}^{-} + K_{k} (y_{k} - H_{k}\hat{x}_{k}^{-}) \\ P_{k}^{+} &=& (I - K_{k}H_{k})P_{k}^{-}(I-K_{k}H_{k})^{T} + K_{k}R_{k}K_{k}^{T} \\ &=& [(P_{k}^{-})^{-1} + H_{k}^{T}R_{k}^{-1}H_{k}]^{-1} \\ &=& (I - K_{k}H_{k})P_{k}^{-} \end{eqnarray*}

这是个迭代的过程，在迭代过程中\(k=1,2,\ldots\)向前演进。

接下来是matlab代码实现：

 1: % sampling interval
 2: T = 5;
 3: % position measurement standard deviation
 4: sigma = 30;
 5: R = sigma^2;
 6: % initial state estimate uncertainty
 7: P0 = [100 0 0; 0 10 0; 0 0 1];
 8: H = [1 0 0];
 9: % state transition matrix
10: F = [1 T T*T/2; 0 1 T; 0 0 1];
11: x = [1; 1; 1]; % initial state
12: xhat = x; % initial state estimate

T 是采样间隔。=sigma= 是位置估计的标准差。=R= 是位置估计的方差。

kalman滤波应用

目录

1 无噪牛顿系统

2 matlab实现