8、卡尔曼滤波器：推导、应用与变体

卡尔曼滤波器：推导、应用与变体

1. 卡尔曼滤波器推导

1.1 目标与常见问题

我们的目标是找到估计器 $\hat{z}(t|t)$ 的递归形式。常见的教科书推导方式是先假定递归形式为 $\hat{z}(t + 1|t + 1) = H(t)\hat{z}(t|t) + K(t)m(t) + \alpha(t)$，然后推导 $H(t)$、$K(t)$ 和 $\alpha(t)$ 的最优设置。但这种方法存在两个问题：
1. 直接假定递归形式，只能找到最佳递归形式，不一定能证明该递归形式就是整体的最佳估计器。
2. 错过向读者说明为什么递归形式是最优的机会，即问题结构中是什么因素使得递归形式得以出现。

1.2 推导步骤

推导分为三个部分：
1. 间接估计
- 给定问题：
- $m = Cz + v$，$v \sim N(0, R)$
- $q = Dz + w$，$w \sim N(0, S)$
- $z \sim N(0, P)$
- 静态估计器形式：
- $\hat{z}(m) = \Lambda_{zm}\Lambda_{m}^{-1} m$
- $\hat{q}(m) = \Lambda_{qm}\Lambda_{m}^{-1} m$
- 交叉统计量推导：
- $\Lambda_{zm} = E[z m^{T}] = E[z(Cz + v)^{T}] = PC^{T}$
- $\Lambda_{qm} = E[q m^{T}] = E[(Dz + w)(Cz + v)^{T}] =