在这一节中，我们将推导针对离散时间动力学系统和离散时间量测的离散时间扩展卡尔曼滤波。这种状况在实际中经常遇到。即使基本的动力学系统是连续的，但是扩展卡尔曼滤波通常需要在数字计算机中实现。这就意味着可能没有足够的计算能力来完成连续时间扩展卡尔曼滤波或混合扩展卡尔曼滤波中的积分运算。因此动力学系统通常经过离散化然后利用扩展卡尔曼滤波进行估计。

  假设我们有系统模型如下：

\[
x_k=f_{k-1}(x_{k-1},u_{k-1},\omega_{k-1})\\
y_k=h_k(x_k,\nu_k)
\]
\[
\omega_k\sim(0,Q_k)\\
\nu_k\sim(0,R_k)
\]

  我们对状态方程在\( x_{k-1}=\hat{x} _ {k-1}^+ \)和\( \omega _{k-1}=0 \)点进行泰勒级数展开如下

\[
\begin{align}
x_k&=f_{k-1}(\hat{x}_{k-1}^+,u_{k-1},0)+\frac{ \partial f_{k-1} }{\partial x}|_{\hat{x} _{k-1}^+}(x _{k-1}-\hat{x} _{k-1}^+)+\frac{ \partial f _{k-1} }{\partial \omega}|_{\hat{x} _{k-1}^+}\omega _{k-1}\\
&=F_{k-1}x_{k-1}+[f_{k-1}(\hat{x}_{k-1}^+,u_{k-1},0)-F_{k-1}\hat{x}_ {k-1}^+]+L_{k-1}\omega_{k-1}\\
&=F_{k-1}x_{k-1}+\tilde{u}_{k-1}+\tilde{\omega}_{k-1} \tag{1}
\end{align}
\]

  \(F_{k-1}\)和\(L_{k-1}\)由上式定义。已知信号\(\tilde{u}_k\)和噪声信号\(\tilde{\omega}_k\)定义如下：

\[
\tilde{u}_k=f_k(\hat{x}_k^+,u_k,0)-F_k\hat{x}_k^+\\
\tilde{\omega}_k\sim(0,L_kQ_kL_k^T)
\]

  我们对量测方程在\(x_k=\hat{x}_k^-\)和\(\nu_k=0\)点进行泰勒级数展开如下

\[
\begin{align}
y_k&=h_k(\hat{x}_k^-,0)+\frac{ \partial h_k }{\partial x}|_{\hat{x}_k^-}(x_k-\hat{x}_k^-)+\frac{ \partial h_k }{\partial \nu}|_{\hat{x}_k^-}\nu_k\\
&=H_kx_k+[h_k(\hat{x}_k^-,0)-H_k\hat{x}_k^-]+M_k\nu_k\\
&=H_kx_k+z_k+\tilde{\nu}_k \tag{2}
\end{align}
\]

  \(H_k\)和\(M_k\)由上式定义。已知信号\(z_k\)和噪声信号\(\tilde{\nu}_k\)定义如下：

\[
z_k=h_k(\hat{x}_k^-,0)-H_k\hat{x}_k^-\\
\tilde{\nu}_k\sim(0,M_kQ_kM_k^T)
\]

  我们有式(1)的线性状态方程和式(2)的线性量测方程。那意味着我们可以使用标准的卡尔曼滤波方程估计状态。也就是下式的离散扩展卡尔曼滤波方程：

\[
P_k^- = F_{k-1} P_{k-1}^+ F_{k-1}^T+L_{k-1}Q_{k-1}L_{k-1}^T\\
K_k=P_k^-H_k^T(H_kP_k^-H_k^T+M_kR_kM_k^T)^{-1}\\
\hat{x}_k^-=f_{k-1}(\hat{x}_{k-1}^+,u_{k-1},0)\\
z_k=h_k(\hat{x}_k^-,0)-H_k\hat{x}_k^-\\
\hat{x}_k^+=\hat{x}_k^-+K_k(y_k-H_k\hat{x}_k^–z_k)=\hat{x}_k^-+K_k(y_k-h_k(\hat{x}_k^-,0))\\
P_k^+ =(I-K_kH_k)P_k^-
\]

扩展卡尔曼滤波总结

  
  1.动态系统方程如下：

\[
x_k=f_{k-1}(x_{k-1},u_{k-1},\omega_{k-1})\\
y_k=h_k(x_k,\nu_k)
\]
\[
\omega_k\sim(0,Q_k)\\
\nu_k\sim(0,R_k)
\]

  2.滤波器初始化条件如下：

\[
\hat{x}_0^+=E[x_0]\\
P_0^+=E[(x_0-\hat{x}_0^+)(x_0-\hat{x}_0^+)^T]
\]

  3.卡尔曼滤波每一步计算如下，其中\(k=1,2,\cdots\)

  a)计算以下偏微分：

\[
F_{k-1}=\frac{ \partial f_{k-1} }{\partial x}|_{\hat{x}_{k-1}^+}\\
L_{k-1}=\frac{ \partial f_{k-1} }{\partial \omega}|_{\hat{x}_{k-1}^+}
\]

  b)状态估计和估计误差协方差的时间更新如下：

\[
\begin{align}
\hat{x}_k^-&=f_{k-1}(\hat{x}_{k-1}^+,u_{k-1},0) \tag{11}\\
P_k^- &= F_{k-1} P_{k-1}^+ F_{k-1}^T+L_{k-1}Q_{k-1}L_{k-1}^T \tag{12}
\end{align}
\]

  c)计算以下偏微分：

\[
H_k=\frac{ \partial h_k }{\partial x}|_{\hat{x}_k^-}\\
M_k=\frac{ \partial h_k }{\partial \nu}|_{\hat{x}_k^-}
\]

  d)状态估计的最测更新和估计误差协方差的更新如下：

\[
\begin{align}
K_k&=P_k^-H_k^T(H_kP_k^-H_k^T+M_kR_kM_k^T)^{-1} \tag{13}\\
\hat{x}_k^+&=\hat{x}_k^-+K_k(y_k-h_k(\hat{x}_k^-,0)) \tag{14}\\
P_k^+ &=(I-K_kH_k)P_k^- \tag{15}
\end{align}
\]