Kalman Filter的推导与应用（二）

（二）Kalman Filter公式推导

考虑一个用状态空间模型描述的动态系统

$$X(t+1)=\Phi X(t) + \Gamma W(t)$$ $$Y(t)=H X(t) + V(t)$$ 其中， $t$ 为时间，系统在时刻 $t$ 的状态为 $X(t) \in R^n$ ；Y(t)为观测信号； $W(t) \in R^r$ 为输入的噪声， $V(t)\in R^m$ 为观测噪声。由于不同问题的状态空间描述是不一样的，因此只有在确定状态空间后才可以得到递推公式（这是我自己的看法，不知道对不对，可能递推公式还是一样，不过根据问题来推一遍公式总不会有错的，只是麻烦点）。

【假设1】

• $W(t)$ 和 $V(t)$ 是均值为零，方差各为 $Q$ 和 $R$ 的不相关白噪声

【假设2】

• 初始状态 $X(0)$ 与 $W(t)$ 和 $V(t)$ 不相关，而且
  $$E[X(0)] = \mu_0,E[(X(0)-\mu_0)(X(0)-\mu_0)^T] = P_0$$

推导

• 问题重述

卡尔曼滤波是一个基于观测信号 $\{Y(1),Y(2),...,Y(t) \}$ ，求状态 $X(k)$ 的线性最小方差估计值的问题 $\hat X(k|t)$ ，而且这个问题的目标函数是

$$J=E[(X(k)-\hat X(k|t))^T(X(k)-\hat X(k|t))]$$ 那么由之前的定义1.1可以得到，卡尔曼滤波问题可以归为求射影
$$\hat X(k|t) = proj(X(k)|Y(1),Y(2),...,Y(t))$$ 由定义1.5可以得到递推公式为
$$\hat X(t+1|t+1) = \hat X(t+1|t)+K(t+1)\epsilon (t+1)$$ $$K(t+1)=E[X(t+1)\epsilon^T(t+1)]\{ E[\epsilon (t+1)\epsilon^T(t+1)]\}^{-1}$$ 其中 $K(t+1)$ 为卡尔曼滤波器增益。

• 对状态方程求射影

以 $w$ 表示观测信号，对状态方程 $X(t+1)=\Phi X(t) + \Gamma W(t)$ 两边取射影有（射影运算具有分配律，跟向量点乘一样）

$$proj(X(t+1)|w) = \Phi proj( X(t)|w)+\Gamma proj(W(t)|w)$$ $$\hat X(t+1|t) = \Phi \hat X(t|t)+\Gamma proj(W(t)|w)$$ 迭代求解状态方程后获得原状态方程的另一种表示