卡尔曼滤波的推导

1 最小二乘法

在一个线性系统中，若\(x\)为常量，是我们要估计的量，关于\(x\)的观测方程如下：

\[ y = Hx + v \tag{1.1}\]

\(H\)是观测矩阵（或者说算符），\(v\)是噪音，\(y\)是观察量。若关于\(x\)的最佳估计为\(\hat{x}\),误差可定义为\(\epsilon_y\):

\[ \epsilon_y = y - H\hat{x} \tag{1.2}\]

定义代价函数\(J\)为：

\[ J = \epsilon_{y1}^2 + \epsilon_{y2}^2 + ... + \epsilon_{yk}^2 \tag{1.3}\]

将方程\((1.2)\)代入\((1.3)\)可得：

\[ J=\left(y-H\hat{x}\right)^T\left(y-H\hat{x}\right)=y^Ty-x^TH^Ty-y^TH\hat{x}+\hat{x}^TH^TH\hat{x} \tag{1.4} \]

使代价函数\(J\)最小的就是最佳估计，对\(J\)求偏导，并使之为0：

\[ \frac{\partial J}{\partial \hat{x}}=-2y^TH+2\hat{x}^TH^TH=0 \tag{1.5}\]

解得：

\[ \hat{x} = \left(H^TH\right)^{-1}H^Ty \tag{1.6} \]

2 递推最小二乘法

线性系统的递推估计值可以写成以下形式：

\[ y_k=H_kx+v_k \tag{2.1} \]
\[ \hat{x}_k = \hat{x}_{k-1} + K_k\left(y_k-H_k\hat{x}_{k-1}\right) \tag{2.2} \]

其中是\(K_k\)待定的增益矩阵，\(y_k-H_k\hat{x}_{k-1}\)为修正项。先考虑线性递推估计器的估计误差均值,要注意的是这里和式\((1.2)\)有所不同，但本质都是一样东西：

\[ \begin{split} E\left(\epsilon_{x,k}\right) &= E\left(x-\hat{x}_k\right) \cr &= E\left(x- \hat{x}_{k-1}-K_k\left(y_k-H_k\hat{x}_{k-1}\right)\right) \newline &= E\left(\epsilon_{x,k-1}-K_k\left(H_kx+v_k-H_k\hat{x}_{k-1}\right)\right) \cr &= E\left(\epsilon_{x,k-1}-K_kH_k\left(x-\hat{x}_{k-1}\right)-K_kv_k\right) \cr &=\left(I-K_kH_k\right)E\left(\epsilon_{x,k-1}\right)-K_kE\left(v_k\right) \end{split} \tag{2.3} \]

类似地，代价函数\(J_k\)表达为：

\[ J_k=E(\epsilon_{x1,k}^2+\epsilon_{x2,k}^2+...+\epsilon_{xn,k}^2)=E(\epsilon_{x,k}^T\epsilon_{x,k})=E(Tr(\epsilon_{x,k}\epsilon_{x,k}^T))=Tr(P_k) \tag{2.4}\]

其中\(P_k\)为估计误差的协方差矩阵，利用式\((2.3)\)可以得到：

\[ \begin{split} P_k &= E\left(\epsilon_{x,k}\epsilon_{x,k}^T\right) \newline &=E\left\{\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]^T\right\} \cr &=(I-K_kH_k)E(\epsilon_{x,k}\epsilon_{x,k}^T)(I-K_kH_k)^T - K_kE(v_k\epsilon_{x,k-1}^T) \cr & \quad -(I-K_kH_k)E(\epsilon_{x,k-1}v_k^T)K_k^T+K_kE(v_kv_k^T)K_k^T \end{split} \tag{2.5} \]

由于\(\epsilon_{x,k-1}\)与\(v_k\)是相互独立的，所以有：

\[ E(v_k\epsilon_{x,k-1}^T)=E(v_k)E(\epsilon_{x,k-1})=0 \tag{2.6} \]

令\(R_k=E(v_kv_k^T)\)为噪声的协方差矩阵，式\((2.5)\)变成：

\[ P_k = (I-K_kH_k)P_{k-1}(1-K_kH_k)^T + K_kR_kK_k^T \tag{2.7} \]

我们的目标是求出待定的增益矩阵\(K_k\)，对\(J_k\)求偏导，并使之为0可得：

\[ \frac{\partial J_k}{\partial K_k}=2(I-K_kH_k)P_{k-1}(-H_k)^T+2K_kR_k = 0 \tag{2.8} \]

可\(K_k\)解得以下：

\[ \begin{split} (I-K_kH_k)P_{k-1}H_k^T=K_kR_k \cr K_k(R_k+H_kP_{k-1}H_k^T)=P_{k-1}H_k^T \cr K_k=P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T\right)^{-1} \end{split} \tag{2.9} \]

递推最小二乘法总结如下:

a. 初始估计值：

\[ \hat{x}_0 = E(x) \tag{2.10} \]
\[ P_0 = E\left[(x - \hat{x}_0)(x-\hat{x}_0)^T\right] \tag{2.11}\]

b. 对于k=1,2,3…:
假设线性系统的观测方程如下：

\[ y_k = H_kx + v_k \tag{2.12} \]

其中是\(v_k\)均值为0，协方差矩阵为\(R_k\)的随机变量，每步测量的噪声都是相互独立的，则矩阵为\(R_k\)对角矩阵，也就是说测量的噪声为白噪声。

更新方程如下：
\[K_k = P_{k-1}H_k^T\left(R_k + H_kP_{k-1}H_k^T\right)^T \tag{2.13}\]
\[\hat{x}_k = \hat{x}_{k-1} + K_k(y_k - H_k\hat{x}_{k-1}) \tag{2.14}\]
\[ P_k = (I-K_kH_k)P_{k-1}(I-K_kH_k)^T+K_kR_kK_k^T \tag{2.15}\]

3 递推最小二乘法的更简洁表示

首先要为估计误差协方差表达式寻找一个新的形式。把式\((2.14)\)代入式\((2.15)\)中，可以得到：

\[ P_k=\left[I-P_{k-1}H_k^TH_k\right]P_{k-1}\left[I-P_{k-1}H_k^TH_k\right]^T + K_kR_kK_k^T \tag{3.1}\]

其中\(S_k=R_k+H_kP_{k-1}H_k^T\),再把\(K_k\)代入并展开可得(考虑到\(S_k\)与\(P_k\)的对称性)：

\[ \begin{split} P_k &= P_{k-1} - 2P_{k-1}H_k^TS_k^{-1}H_kP_{k-1}+P_{k-1}H_k^TS_k^{-1}H_kP_{k-1}H_k^TS_k^{-1}H_kP_{k-1} \cr &\quad + P_{k-1}H_k^TS_k^{-1}R_kS_k^{-1}H_kP_{k-1} \cr &=P_{k-1} - 2P_{k-1}H_k^TS_k^{-1}H_kP_{k-1}+P_{k-1}H_k^TS_k^{-1}S_kS_k^{-1}H_kP_{k-1} (合并最后两项）\cr &=P_{k-1}-P_{k-1}H_k^TS_k^{-1}H_kP_{k-1} \end{split} \tag{3.2} \]

由\(K_k=P_{k-1}H_k^TS_k^{-1}\)可以得到：

\[ P_k = (I - K_kH_k)P_{k-1} \tag{3.3}\]

对于式\((3.2)\)，可以得到以下的表达：

\[ P_k = P_{k-1} - P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T\right)^{-1}H_kP_{k-1} \tag{3.4}\]

两边求逆，并用矩阵反演定理\(\left(A-BD^{-1}C\right)^{-1}=A^{-1}+A^{-1}B\left(D-CA^{-1}B\right)^{-1}CA^{-1}CA^{-1}\)可以得到：

\[ \begin{split} P_k^{-1} &= P_{k-1}^{-1}+P_{k-1}^{-1}P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T-H_kP_{k-1}P_{k-1}^{-1}P_{k-1}H_k^T\right)^{-1}H_kP_{k-1}P_{k-1}^{-1} \cr &=P_{k-1}^{-1}+H_k^TR_k^{-1}H_k \end{split} \tag{3.5} \]

由式\((2.13)\)可得：

\[ \begin{split} K_k&=P_kP_k^{-1}P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T\right)^{-1} \cr &=P_k\left(P_{k-1}^{-1}+H_k^TP_{k-1}^{-1}H_k\right)P_{k-1}H_k^T(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_k(H_k^T+H_k^TR_k^{-1}H_kP_{k-1}H_k^T)(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_kH_k^T(I+R_k^{-1}H_kP_{k-1}H_k^T)(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_kH_k^TR_k^{-1}(R_k+H_kP_{k-1}H_k^T)(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_kH_k^TR_k^{-1} \end{split} \tag{3.6} \]

可以得到式\((2.13)\)的简洁形式是\((3.3)\)，式\((2.15)\)的简洁形式是\((3.6)\)。

4 协方差的传播

对于离散时间的线性系统，可以以下式子表达：

\[ x_k = F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1} \tag{4.1} \]

其中，\(u_k\)是已知的输入，\(w_k\)是零均值的白噪声，协方差为\(Q_k\)。那么状态\(x_k\)的均值\(\overline{x}_k\)随时间有怎样的变化？如果对式\((4.1)\)两边取期望将会得到状态随着时间的传播方程：

\[ \overline{x}_k = E(x_k) =F_{k-1}\overline{x}_{k-1}+G_{k-1}u_{k-1} \tag{4.2} \]

\(x_k\)的协方差随时间会有怎么样的变化？通过式\((4.1)\)和式\((4.2)\)可以得到：

\[ \begin{split} &\quad (x_k-\overline{x}_k)(x_k - \overline{x}_k)^T \cr &=(F_{k-1}\overline{x}_{k-1}+G_{k-1}u_{k-1}-\overline{x}_k)(F_{k-1}\overline{x}_{k-1}+G_{k-1}u_{k-1}-\overline{x}_k)^T \cr &=[F_{k-1}(x_{k-1}-\overline{x}_{k-1})+w_{k-1}][F_{k-1}(x_{k-1}-\overline{x}_{k-1})+w_{k-1}]^T \cr &=F_{k-1}(x_{k-1}-\overline{x}_{k-1})(x_{k-1}-\overline{x}_{k-1})^TF_{k-1}^T+w_{k-1}w_{k-1}^T+\cr &\quad F_{k-1}(x_{k-1}-\overline{x}_{k-1})w_{k-1}^T+w_{k-1}(x_{k-1}-\overline{x}_{k-1})^TF_{k-1}^T \cr \end{split} \tag{4.3} \]

对上述的式子求期望就能到得协方差。由于\((x_{k-1}-\overline{x}_{k-1})\)与\(w_{k-1}\)相互独立，所以它们之间的协方差为0，因些可以得到：

\[ P_k=E((x_k-\overline{x}_k)(x_k - \overline{x}_k)^T)=F_{k-1}P_{k-1}F_{k-1}^T+Q_{k-1} \tag{4.4}\]

这个就是协方差的传播方程。

5 离散卡尔曼滤波的推导

终于来我们最重要的环节了，就是要推导卡尔曼滤波。和之前一样，先来假设一个线性离散系统，如下：

\[ \begin{split} x_k &= F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1} \cr y_k &= H_kx_k+v_k \cr \end{split} \tag{5.1} \]

\(w_k\)与\(v_k\)是零均值且相互独立的噪声，有已知的协方差矩阵\(Q_k\)和\(R_k\)，可以有以下：

\[ \begin{split} w_k \quad &\tilde{} \quad (0, Q_k) \cr v_k \quad &\tilde{} \quad (0, R_k) \cr E[w_kw_k^T] &= Q_k\delta_{k-j} \cr E[v_kv_k^T] &= R_k\delta_{k-j} \cr E[v_kw_k^T] &= 0 \cr \end{split} \tag{5.2} \]

其中\(\delta_{k-j}\)是\(Kronecker-\delta\)函数，是信号与处理中常用的单位脉冲函数，如果\(k=j\)，那么\(\delta_{k-j}=1\)，否则等于0。我们的目的是在已知的系统动态方程和带噪声测量{\(y_k\)}的基础上估计状态量\(x_k\)。对于状态估计可以的信息量，取决于我们要解决问题的本身。

如果我们利用包括k时刻和k时刻之前的测量信息来估计\(x_k\)，那么能得到一个后验估计\(\hat{x}_k^+\)，上标的“+”号表示这个估计的后验，获取后验估计\(\hat{x}_k^+\)是在k时刻与k时刻之前的测量值的条件下计算\(x_k\)的期望值，即：

\[ \hat{x}_k^+ = E\left[x_k | y_1, y_2,...,y_k\right]=后验估计 \tag{5.3} \]

如果利用k时刻之前但不包括k时刻的测量值来估计\(x_k\)，那么能得到一个先验估计\(\hat{x}_k^-\),同样可以计算相应的期望值得到先验估计：

\[ \hat{x}_k^- = E\left[x_k | y_1, y_2,...,y_{k-1}\right]=先验估计 \tag{5.4} \]

如果我们已经知道了k时刻之后的测量值，可以利用这些信息对\(x_k\)进行估计，这个叫做平滑估计，即：

\[ \hat{x}_{k|k+N} = E\left[x_k | y_1, y_2,...,y_{k+N}\right]=平滑估计 \tag{5.5} \]

如果我们已经知道了k时刻之前有\(M\)个测量值未知，可以利用这些信息对\(x_k\)进行估计，这个叫做预测估计，即：

\[ \hat{x}_{k|k-M} = E\left[x_k | y_1, y_2,...,y_{k-M}\right]=预测估计 \tag{5.6} \]

从利用信息的多少可以看到关于\(x_k\)的最优估计次序依次是平滑估计、后验估计、先验估计、预测估计，因为用多的信息量肯定能得到不比信息量少的结果差。

用\(\hat{x}_0^+\)来表示没有使用任何测量结果（信息）得到的初始值，第一个测量值是在时间\(k=1\)时测量得到的。我们可以用初始状态\(x_0\)的期望来得到\(\hat{x}_0^+\)，即有：

\[ \hat{x}_0^+= E(x_0) \tag{5.7}\]

类似地，定义\(P_k^-\)与\(P_k^+\)分别是先验估计与后验估计的协方差，即：

\[ \begin{split} P_k^+=E\left[(x_k-\hat{x}_k^+)(x_k-\hat{x}_k^+)^T\right] \newline P_k^-=E\left[(x_k-\hat{x}_k^-)(x_k-\hat{x}_k^-)^T\right] \newline \end{split} \tag{5.8} \]

系统以\(\hat{x}_0^+\)为最优的初始状态，那么怎么推算到下一个时间点（也就是\(\hat{x}_1^-\)）呢？参加式\((4.2)\)的状态传播方程，可以得到：

\[ \hat{x}_1^- = F_0\hat{x}_0^+ + G_0u_0 \tag{5.9} \]

上述的方程可以推广到更一般的情况，即为状态\(\hat{x}\)的时间更新方程：

\[ \hat{x}_k^- =F_{k-1}\hat{x}_{k-1}^++G_{k-1}u_{k-1} \tag{5.10} \]

同理由式\((4.4)\)可以得到协议差\(P\)的时间更新方程：

\[ P_k^- = F_{k-1}P_{k-1}^+F_{k-1}^T+Q_{k-1} \tag{5.11}\]

上面我们已经得到状态\(\hat{x}\)与协方差\(P\)的时间更新方程，这个更新方程是不用测量值来参与的，如果在\(k\)时刻测得到了\(y_k\)，那么如何在\(\hat{x}_k^-\)与\(y_k\)的基础上得到后验估计\(\hat{x}_k^+\)呢？参考递推最小二乘法，我同样可以得到测量的更新方程：

\[ \begin{split} K_k &= P_k^-H_k^T(H_kP_k^-H_k^T+R_k)^{-1}=P_k^+H_k^TR_k^{-1} \cr \hat{x}_k^+ &= \hat{x}_k^- + K_k(y_k - H_k\hat{x}_k^-) \cr p_k^+ &= (I-K_kH_k)P_k^-(I-K_kH_k)^T+K_kR_kK_k^T \cr &=[(P_k^-)^{-1}+H_k^TR_k^{-1}H_k]^{-1} = (I - K_kH_k)P_k^- \cr \end{split} \tag{5.12} \]

其中\(\hat{x}_k\)和\(P_k\)叫做测量更新方程，\(K_k\)叫做卡尔曼滤波增益。下表是最小二乘估计的递推形式与卡尔曼滤波器的对比关系：

递推最小二乘估计	卡尔曼滤波器
\(\hat{x}_{k-1}\)是\(y_k\)处理前的估计值	\(\hat{x}_k^-\)先验估计
\(P_{k-1}\)是\(y_k\)处理前的协方差估计值	\(P_k^-\)先验协方差估计
\(\hat{x}_k\)是\(y_k\)处理后的估计值	\(\hat{x}_k^+\)后验估计
\(P_k\)是\(y_k\)处理后的协方差估计值	\(P_k^+\)后验协方差估计

离散卡尔曼滤波的总结如下：

1. 动态线性系统的方程如下：

\[ \begin{split} & x_k = F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1} \cr & y_k = H_kx_k+v_k \cr & E[w_kw_k^T] = Q_k\delta_{k-j} \cr & E[v_kv_k^T] = R_k\delta_{k-j} \cr & E[v_kw_k^T] = 0 \cr \end{split} \tag{5.13} \]

2. 卡尔曼滤波器的初始化如下：

\[ \begin{split} \hat{x}_0^+ &= E(x_0) \newline P_0^+ &=E\left[(x_0-\hat{x}_0^+)(x_0-\hat{x}_0^+)^T\right] \newline \end{split} \tag{5.14} \]

3. 卡尔曼滤波器的时间更新方程与测量更新方程如下， \(k=1,2,3,...\)：

\[ \begin{split} P_k^- &= F_{k-1}P_{k-1}^+F_{k-1}^T+Q_{k-1} \cr K_k &= P_k^-H_k^T(H_kP_k^-H_k^T+R_k)^{-1}=P_k^+H_k^TR_k^{-1} \cr \hat{x}_k^- &=F_{k-1}\hat{x}_{k-1}^++G_{k-1}u_{k-1} \cr \hat{x}_k^+ &= \hat{x}_k^- + K_k(y_k - H_k\hat{x}_k^-) \cr p_k^+ &= (I-K_kH_k)P_k^-(I-K_kH_k)^T+K_kR_kK_k^T \cr &=[(P_k^-)^{-1}+H_k^TR_k^{-1}H_k]^{-1} \cr &= (I - K_kH_k)P_k^- \cr \end{split} \tag{5.15} \]

从上述的方程可以看到：1). \(P_k^-\)、\(P_k^+\)与\(K_k\)是不取决于量测值\(y_k\)的，所以可以脱机运算，预先计算好这些量可以使计算机达到实时运行的需求；2). 如果\(x_k\)是一个常量，那么\(F_k=I\)、\(Q_k=0\)和\(u_k=0\)，这种情况下卡尔曼滤波就变成了最小二乘估计，反过来说卡尔曼滤波是最小二乘法的推广，本质上是通过减少动态系统状态的方差来达到最优的估计；3).要注意的是\(p_k^+\)的第一个形式比第三个形式在数值计算上更加稳定、鲁棒性更好，因为第一个表达式只要\(p_k^-\)是对称的正定矩阵，那么\(p_k^+\)也一定是对称的正定矩阵。