卡尔曼滤波的推导

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_khat{x}_{k-1}$为修正项。先考虑线性递推估计器的估计误差均值,要注意的是这里和式$(1.2)$有所不同，但本质都是一样东西：

$$
begin{split}
Eleft(epsilon_{x,k}right) &= Eleft(x-hat{x}_kright) cr
&= Eleft(x- hat{x}_{k-1}-K_kleft(y_k-H_khat{x}_{k-1}right)right) newline
&= Eleft(epsilon_{x,k-1}-K_kleft(H_kx+v_k-H_khat{x}_{k-1}right)right) cr
&= Eleft(epsilon_{x,k-1}-K_kH_kleft(x-hat{x}_{k-1}right)-K_kv_kright) cr
&=left(I-K_kH_kright)Eleft(epsilon_{x,k-1}right)-K_kEleft(v_kright)
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 &= Eleft(epsilon_{x,k}epsilon_{x,k}^Tright) newline
&=Eleft{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]^Tright} cr
&=(I-K_kH_k)E(epsilon_{x,k}epsilon_{x,k}^T)(I-K_kH_k)^T - K_kE(v_kepsilon_{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^Tleft(R_k+H_kP_{k-1}H_k^Tright)^{-1}
end{split} tag{2.9}
$$

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

初始估计值：

$$ \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}$$

对于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}Cright)^{-1}=A^{-1}+A^{-1}Bleft(D-CA^{-1}Bright)^{-1}CA^{-1}CA^{-1}$可以得到：

$$
begin{split}
P_k^{-1} &= P_{k-1}^{-1}+P_{k-1}^{-1}P_{k-1}H_k^Tleft(R_k+H_kP_{k-1}H_k^T-H_kP_{k-1}P_{k-1}^{-1}P_{k-1}H_k^Tright)^{-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^Tleft(R_k+H_kP_{k-1}H_k^Tright)^{-1} cr
&=P_kleft(P_{k-1}^{-1}+H_k^TP_{k-1}^{-1}H_kright)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}^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_kdelta_{k-j} cr
E[v_kv_k^T] &= R_kdelta_{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^+=Eleft[(x_k-hat{x}_k^+)(x_k-hat{x}_k^+)^Tright] newline
P_k^-=Eleft[(x_k-hat{x}_k^-)(x_k-hat{x}_k^-)^Tright] 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_khat{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_kdelta_{k-j} cr
& E[v_kv_k^T] = R_kdelta_{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^+ &=Eleft[(x_0-hat{x}_0^+)(x_0-hat{x}_0^+)^Tright] 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_khat{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^+$也一定是对称的正定矩阵。