当观测已成过去，状态又该如何更新？

组合导航里面，由于各种传输延时也好，传感器特性也好，GNSS/图像等观测的时间戳可能会比当前时间戳更早，这个时候如何进行状态量的更新？

设观测时间为\(t_h\)，当前状态时间为\(t_s\)，有几种方法来处理这个事情

1 摆烂法

人生苦短，直接开摆，如果\(\mid t_{h}-t_{s} \mid\)过大，则不用该观测，否则直接把\(t_{h}\)时刻的观测当\(t_{s}\)时刻用。

这样可能会造成轨迹在局部范围内震荡。

2 回溯 + 重新递推

如果不能对当前状态直接进行观测更新，那为何不对历史状态进行观测更新？

可以保存若干历史时刻的状态量的预测值，以及对应的协方差。设有n个历史状态，可以根据观测的时间戳\(t_{h}\)找到时间戳最接近的一帧的状态\(x_{h}\)及其对应的协方差\(P_{h}\)。则利用卡尔曼滤波的更新公式进行更新，得到后验状态\(\hat{x_{h}}\)。随后，再把后验状态重新递推与更新到当前时刻。

这种做法较为简单，且理论上没有问题，但是不光需要保存若干历史状态及其对应的协方差，还需要保存对应的传感器的值，这样才能从\(t_h\)时刻递推到当前时刻，既消耗内存，计算量也相对较大（相对于下面两种方法）。

3 Kalman filter with delayed measurement

文献Optimal State Estimation -Kalman, H infinity, and Nonlinear Approaches里提出了一种方法，用来描述历史时刻的观测对当前时刻的状态的影响，这里大致描述一下流程。
假设一个离散线性系统其状态递推与观测公式为

\[\begin{align} x(k) &= F(k-1)x(k-1) + w (k, k-1) \tag{1} \\ y(k) &= H(k)x(k) + v(k) \end{align} \]

其中\(w(k)\)为process model从\(k-1\)转移到\(k\)累计的噪声，\(v(k)\)为观测噪声。
设当前时刻为\(k\)，如果收到的观测为\(k_{0}\)时刻，且\(k_{0} < k\)，如何根据\(k_{0}\)时刻的观测\(y(k_{0})\)来更新\(k\)时刻的状态\(x(k)\)呢？
回顾卡尔曼的预测与更新公式为：

\[\begin{aligned} \hat{x}^{-}(k) &= F(k-1)\hat{x}^{+}(k-1) \\ P^{-}(k) &= F(k-1)P^{+}(k-1)F^{\top}(k-1) + Q(k) \\ P_{xy} &= P^{-}(k)H^{\top}(k) \\ P_{y} &= S_{y} = H(k)P^{-}(k)H^{\top}(k) + R(k) \\ K(k) &= P_{xy}P_{y}^{-1} \\ \hat{x}^{+}(k) &= \hat{x}^{-}(k) + K(k)(y(k) - H(k)\hat{x}^{-}(k)) \\ P^{+}(k) &= P^{-}(k) - K(k)P_{y}K^{\top}(k) \\ &= P^{-}(k) - P_{xy}P_{y}^{-1}P_{xy}^{\top} \end{aligned} \]

那么，在收到\(k_{0}\)时刻的观测后，后验状态可以表示为

\[\hat{x}(k,k_{0})=\hat{x}(k)+P_{xy}(k,k_{0})S^{-1}(k_{0})[y(k_{0})-H(k_{0})\hat{x}(k_{0},k)] \]

其中\(\hat{x}(k), y(k_{0}), H(k_{0})\)均已知，只需要求得\(P_{xy}(k, k_{0}), S(k_{0}), \hat{x}_{k_{0}, k}\)即可。

3.1 \(\hat{x}_{k_{0}, k}\)

这个值的物理意义是，已知k时刻的后验状态\(\hat{x}_{k}\)，如何求得\(k_{0}\)时刻的后验状态？这个问题乍一看似乎有点无意义，因为递推的时候本就是先求得\(k_{0}\)时刻的后验，再一步步推到\(k\)时刻的。但是那个时候求得的后验是\(\hat{x}_{k_{0}}\)，而\(\hat{x}_{k_{0},k}\)则要求我们考虑\(k_{0} \dots k\)时候所有的观测后，给出\(k_{0}\)的后验状态，这两者有所区别。

\[\hat{x}_{k_{0}, k} = E(x(k_{0}) | Y(k)) \tag{2} \]

其中\(Y(k)\)为\(k_{0}\dots k\)时刻所有的观测集合。

考虑到公式(1)，可以将其推广到从\(k_{0}\)转移到\(k\)时刻的方程：

\[x(k) = F(k, k_{0})x(k_{0}) + w(k, k_{0}) \]

则可以得到$$x(k_{0}) = F(k_{0}, k)[x(k)-w(k, k_{0})]$$
其中\(F(k_{0}, k) = F^{-1}(k, k_{0})\)
那么公式(2)可以写为：

\[\begin{align} E(x(k_{0}) | Y(k)) &= F(k_{0}, k)E[x(k) - w(k, k_{0})| Y(k)] \\ &= F(k_{0}, k)[\hat{x}^{-}(k) - \hat{w}(k, k_{0})] \end{align} \]

其中\(\hat{w}(k, k_{0})\)为process model从\(k_{0}\)到\(k\)的累计噪声的期望。这里省略书中的推导步骤，直接给出

\[\begin{align} \hat{w}(k,k_{0})&=\quad\hat{w}^{-}(k,k_{0})+Q(k,k_{0})H^{T}(k)S^{-1}(k)r(k) \\ &=\quad Q(k,k_{0})H^{T}(k)S^{-1}(k)r(k) \end{align} \]

其中\(r(k) = y(k) - H(k)\hat{x}^{-}(k)\)为\(k\)时刻的观测残差，\(S(k)\)为其协方差。最终有

\[\hat{x}_{k_{0}, k} = E[x(k_0)|Y(k)]=F(k_0,k)\left[\hat{x}(k)-Q(k,k_0)H^T(k)S^{-1}(k)r(k)\right] \]

3.2 \(S(k_{0})\)

首先计算回溯到\(k_{0}\)时刻的状态量的协方差

\[\begin{aligned}P(k_{0},k)&= \mathrm{Cov}[x(k_{0})|Y(k)]\\ &= F(k_{0},k)\mathrm{Cov}[x(k)-w(k,k_{0})[Y(k)]F^{T}(k_{0},k)\\&= F(k_{0},k)\Big\{\mathrm{Cov}[x(k)|Y(k)]-\mathrm{Cov}[x(k),w(k,k_{0})|Y(k)]-\\&\mathrm{Cov}^T[x(k),w(k,k_0)|Y(k)]+\mathrm{Cov}[w(k,k_0)|Y(k)]\big\}F^T(k_0,k)\\&= F(k_{0},k)\left\{P^{+}(k)-P_{xw}(k,k_{0})-P_{xw}^{T}(k,k_{0})+\right\}\\&P_w(k,k_0)\} F^T(k_0,k)\end{aligned} \]

则有

\[\begin{aligned}S(k_{0})&= \mathrm{Cov}[y(k_{0})|Y(k)]\\&= E\left\{[H(k_{0})x(k_{0})+v(k_{0})][H(k_{0})x(k_{0})+v(k_{0})]^{T}|Y(k)\right\}\\&= H(k_{0})P(k_{0},k)H^{T}(k_{0})+R(k_{0})\end{aligned} \]

3.3 \(P_{xy}(k, k_{0})\)

直接推导，有

\[\begin{aligned}P_{xy}(k,k_{0})&= \mathrm{Cov}[x(k),y(k_{0})|Y(k)]\\&= \mathrm{Cov}\left\{x(k),H(k_{0})F(k_{0},k)[x(k)-w(k,k_{0})]+v(k_{0})|Y(k)\right\}\\&= [P^{+}(k)-P_{xw}(k,k_{0})]F^{T}(k_{0},k)H^{T}(k_{0})\end{aligned} \]

3.4 总结

总结一下，设当前时刻为\(k\)，如果收到的观测为\(k_{0}\)时刻，且\(k_{0} < k\)，则可以按照以下步骤更新状态量以得到\(\hat{x}(k, k_{0})\)与\(P(k, k_{0})\)：

1. 将当前时刻的状态估计回溯到\(k_0\)时刻：

\[\begin{align} S(k) &= H(k)P^{-}(k)H^\top (k) + R(k) \\ \hat{x}(k_{0}, k) &= F(k_{0}, k)[\hat{x}(k)-Q(k, k_{0})H^\top (k)S^{-1}(k)r(k)] \end{align}\]

2. 计算回溯到\(k_{0}\)时刻后的状态的协方差：

\[\begin{aligned} P_w(k,k_0)&= Q(k,k_0)-Q(k,k_0)H^T(k)S^{-1}(k)H(k)Q(k,k_0)\\ P_{\boldsymbol{x}\boldsymbol{w}}(k,k_0)&= Q(k,k_0)-P^-(k)H^T(k)S^{-1}(k)H(k)Q(k,k_0)\\ P(k_0,k)&=F(k_{0},k)\left\{P(k)-P_{xw}(k,k_{0})-P_{xw}^{T}(k,k_{0})+\right. P_w(k,k_0)F^T(k_0,k) \end{aligned} \]

3. 计算得到\(k_{0}\)时刻的观测后的后验协方差。

\[S(k_{0}) = H(k_{0})P(k_{0}, k)H^\top (k_{0}) +R(k_{0}) \]

4. 计算\(k\)时刻的状态量\(x_{k}\)与\(k_{0}\)时刻的观测\(y(k_{0})\)的协方差。

\[P_{xy}(k,k_0)=[P(k)-P_{xw}(k,k_0)]F^T(k_0,k)H^T(k_0) \]

5. 使用延迟的观测\(y(k_{0})\)来更新状态估计以及其协方差。

\[\begin{align} \hat{x}(k,k_{0})&=\hat{x}(k)+P_{xy}(k,k_{0})S^{-1}(k_{0})[y(k_{0})-H(k_{0})\hat{x}(k_{0},k)]\\ P(k,k_{0})&=P(k)-P_{xy}(k,k_{0})S^{-1}(k_{0})P_{xy}^{T}(k,k_{0}) \end{align} \]