文章以最小二乘法为切入点展开讨论卡尔曼滤波

。卡尔曼滤波与最小二乘法只有很小的区别。

经典最小二乘法解决的问题： 考虑下面的 k 个测量：

y_1=h_1x+v_1\ y_2=h_2x+v_2\ vdots\ y_k=h_kx+v_k

每次测量都有自己的误差. v 不可观测， E(v)=0 。 h,x 是适当维度的向量。最小二乘法通过最小化下面的式子来获得对 x 最有可能的估计：

J=sum_{i=1}^k(y_i-h_ix)^2

hat x =arg min_{x} J

这产生了一个问题，那就是每次测量误差都不一样，不能一概而论，如果每次测量都同样的权重。会导致结果被某些误差比较大的数据带偏。（其实多数情况下误差分布也是不知道的），所以更通用的形式是带权最小二乘法：

J=sum_{i=1}^kig(y_i-h_ixig)^2frac{1}{E(v_i^2)}=sum_{i=1}^kig(y_i-h_ixig)^2frac{1}{sigma_i^2}

其中:

DBig(frac{y_i-h_ix}{sqrt {D(y_i-h_ix)}}Big)=EBig(frac{v_i^2}{D(v_i)}Big)=1

考虑一个有点复杂的问题：

两架飞机 A,B 我们想通过在 A 中观察到的 B 位置信息，近似得到速度，加速度，等信息。

而往往我们自身的状态也在改变。我们对自身状态的改变也是一个带有杂讯的测量。

这一过程可以表示为：

x_k=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1} ag{1}

y_{k}=h_kx_{k}+v_{k} ag{2}

w_,v 是不相关，均值为 0 的杂讯。一般记： hat x 表示对 x 估计

这里，如果我们单看 (2) 式，这其实是一个最小二乘法可以解决的问题。

(2) 式子对应观测数据。比如第 k 次测量 B 的相对位置，也就是 y_k 则可以验证 x_k 。理想情况下，我们估计的 hat x_k 会有： h_khat x_k=y_k .往往测量是有误差的(v_k) 。而在验证之前，因为上一时刻A的速度，位置等状态信息与现在有改变，导致测量的结果与A状态的改变有关。要通过 (1) 式子对历史x_{k-1} 进行状态转移。 这相当于不同参考系观测的数据要处理到同一个参考系。这是因为一个人看到另一个人速度的快慢也跟自身速度有关。

(1) 式是一组状态转移， u_k 是 k 时刻，系统的一个输入。 F_k 则是对上一时刻状态的转移。例如，上一时刻 A 观察到 B 的位置信息到当前的变换。这里G_ku_k 理解为参考系 A 状态的改变。比如产生了一个加速。改变了你观测 B 的相对加速度。

卡尔曼滤波将会以下面的方式来估计状态：

上图中，假设 A 只能得到B 相对于自身的位置和两次测量之间的时间间隔，以及自身位置速度，加速度等信息。

黄色箭头表示我们对 B 的速度，加速度等状态的估计。由于 A的 k-1 时刻到与 k 时刻速度，加速度，位置等信息的不同，所以需要先通过状态转移对历史估计转移到新的估计。再利用 B 最新的位置来 矫正 转移后新的估计。获得 k 时刻 B 状态的估计。

如果没有 (1) 的状态转移，可以直接最小二乘法通过极小化目标函数来求 hat x 。

注意：此时可以假设 A 相当于静止的。且 B 匀加速运动，观测信息 h 是总时间的转移矩阵而不是时间间隔。你可以认为 h=[1,t,frac{1}{2}t^2] x=[x_0,v,a]^T .这个举例只是你便于理解上面 (1),(2) 组成的系统，也可以不用参考。由于 A 的静止，我门忽略掉 (1) .此时就是最小二乘法可以解决的问题。

最小二乘法

J=(H_khat x-Y_k)^TR_k(H_khat x-Y_k)=(hat Y_k-Y_k)^TR_k(hat Y_k-Y_k)

其中， H_k^T=[h_1^T,h_2^T,dots h_k^T],Y_k^T=[y_1,y_2,dots y_k]

R_k=left[egin{matrix}sigma^{-2}_1&0&0&dots&0\ 0&sigma^{-2}_2&0&dots&0\ vdots&vdots&vdots&ddots&vdots\ 0&0&0&dots&sigma^{-2}_k end{matrix} ight]

其中： sigma^2_i=E(v_i^2)

R_k 对每一次观测进行归一化.（这里，其实默认假设了 E(hhat x)=y )

你也可以认为 R_k 表示了权重，即带权最小二乘法（其实权重可以不仅仅限于 sigma ）

为什么权重要是 R_k 呢。单看 (2) 式，不同时刻误差的大小，让我们对某些测量结果不是那么相信。但虽然有不好的测量，但也一定程度上反映了真实的结果。经过归一化后，每次测量误差都变成了均值为 0 ，方差为 1 。而方差相同，意味著两次测量的可信度相同了。

这里其实假设了我们已经知道了测量误差的方差，但现实中我们往往是不知道的。把它假设出来，只是为了便于数学上的说明。如果不知道，那么大可认为方差都是一样。此时第一直觉是设置一个常数。其实大可认为是 1 。因为此时不管取何数值，都不会影响最终结果。因为各项权重都是一样的。

那么一般最小二乘法中， R_k=I imes c,(c为常数) ,按照带权最小二乘法来说，我们默认了 sigma_k^2=c^{-}

经典的求导,令：frac{partial J}{partial hat x}=2hat x^TH_k^TR_kH_k-2Y_k^TR_kH_k=0

则： hat x=(H_k^TR_kH_k)^-H_k^TR_kY_k

一般会记:

P_k=(H_k^TR_kH_k)^- ag{3}

正如标题曲线拟合那样，最小二乘法曲线拟合是不会随时间变化的。而 (1),(2) 式组曾的系统却反应出的是一个会变的 hat x ,也就是说， hat x_k,hat x_{k-1} 是有区别的。所以 y_k,h_k 与 y_{k-1},h_{k-1} 两者测量的 x 已经不一样了。

下面你会看到卡尔曼滤波如何将最小二乘法应用到上面 (1)(2) 组成的系统中。

协方差矩阵

注意： P_k=(H_k^TR_kH_k)^-=E((hat x-x)(hat x-x)^T) ，即为 hat x 的协方差矩阵。这是卡尔曼滤波的关键。

证明：

hat x-x=(H_k^TR_kH_k)^-H_k^TR_kY_k-x

=(H_k^TR_kH_k)^-H^T_kR_kY_k-(H_k^TR_kH_k)^-H^T_kR_k(Y_k-V_k)

=(H_k^TR_kH_k)^-H^T_kR_kV_k

其中： V_k=[v_1,v_2,dots v_k]^T

解释一下：

当我们知道误差 V_k 时，这其实就是确定性求解。

Y_k-V_k=H_kx

H_k^TR_k(Y_k-V_k)=H^T_kR_kH_kx

x = (H_k^TR_kH_k)^-H^T_kR_k(Y_k-V_k)

则：

EBig((hat x-x)(hat x-x)^TBig)=(H_k^TR_kH_k)^-H_k^TR_kEig(V_kV_k^Tig)R_k^TH_k(H_k^TR_kH_k)^-

=(H_k^TR_kH_k)^-=P_k

这里， E(V_kV_k^T)=R_k^- ， E(v_iv_j)=0,i ot= j ,这是因为，不同时刻的测量误差是不相关的。

注意：到这里我们只需要知道了协方差矩阵即可立即有： hat x = P_kH_k^TR_kY_k

进一步令 r_k=sigma^{-2}_k=frac{1}{E(v_k^2)}

P_k=(H^T_kR_kH_k)^-=(H^T_{k-1}R_{k-1}H_{k-1}+h_k^Tr_kh_k)^-=(P_{k-1}^-+h_k^Tr_kh_k)^-

得到一组递推关系。将协方差转为在线计算。

协方差传递

注意，之所以要传递协方差很容易理解。上一时刻与当前时刻的状态是不同。比如参考系 A 位置，速度，加速度的改变。

由于 P_k 就是协方差 (同时也是(H^T_kR_kH_k)^-) 的这一性质，那么当经过 (1) 式，状态转移时，协方差也可以传递。

P_k=EBig((hat x_{k}-x_{k})(hat x_{k}-x_{k})^TBig) =E((F_{k-1}hat x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}-x_{k})(F_{k-1}hat x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}-x_{k})^T)

对于：

E(F_{k-1}hat x_{k-1}+G_{k-1}u_{k-1}+w_{k-1})=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}=x_{k}

解释一下，对于前面的式子中，都是在最小二乘法的背景下来讨论的。那个时候， x 是不随时间变化的。而在加入 (1) 的状态转移后，状态的转移，使得 k-1 时刻的x_{k-1} 与 k 时刻的 x_{k} 有所不同。所以要有下标来做时间上的区分。

那么：

E(F_{k-1}hat x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}-x_{k})

=E(F_{k-1}(hat x_{k-1}-x_{k-1})+w_{k-1})

协方差传递矩阵为：

P_k=F_{k-1}P_{k-1}F^T_{k-1}+Q_{k-1} ag{4}

其中： Q_{k}=E(w_{k}w^T_k) .

这样就方便我们将最小二乘法扩展到 (1),(2) 所组成的系统。

卡尔曼滤波

x^{(-)}_k 表示在不参考 y_k 与 h_k 时，系统对 x_k 的估计。

x_k^{(+)} 表示参考了 y_k,h_k 后，系统对 x_k 的估计。

x^{(-)}_k=F_{k-1}x_{k-1}^{(+)} +G_{k-1}u_{k-1} ag{5}

P_{k}^{(-)}=F_{k-1}P_{k-1}^{(+)}F_{k-1}^T+Q_{k-1} ag{6}

P_k^{(+)}=(P_{k}^{(-)-}+h_{k}^Tr_kh_{k})^- ag{7}

这里，为了区分矩阵的逆。用上标 (-) 表示先验。

但，此时 P_k^{(+)} imes H_k^TR_kY_k ightarrow x_k^{(+)} 这个操作是不对的，按照 A，B 来说，因为参考系 A 在不断的运动。观察到的数据两个时刻差异可能很大。但被观察的 B 却是一个匀加速运动。这就好比你 过去的观察数据 和 现在的观察数据 参考系的 状态一直在改变。所以最好只是利用当前观测数据来修正，不要牵扯过去。其实过去的数据也通过状态转移来传递，例如协方差。但没必要所有都传递。

所以修改最小二乘法。使它只需要使用 h_k,y_k （即最新的观测数据）来更新修正 hat x_k^{(+)} 。

最小二乘法与卡尔曼滤波

在最小二乘法中：

hat x_k=P_kH^T_kR_kY_k 得： H_kR_kY_k=P_k^-hat x_k

因为: P_k=(H_k^TR_kH_k)^-=(P_{k-1}^-+h^T_kr_kh_k)^- ,其中： r_k=frac{1}{sigma_k^2}

得到： P_{k-1}^-=P^-_k-h^T_kr_{k}h_k

整理一下：

hat x_k=P_kH^T(k)Y(k)

=P_kBig(P_{k-1}^-hat x_{k-1}+h^T_kr_ky_kBig)

=P_kBig((P_k^--h^T_kr_kh_k)hat x_{k-1}+h^T_kr_ky_kBig)

=hat x_{k-1}+P_{k}h^T_kr_k(y_k-h_khat x_{k-1})

这样，我们不需要维护 H_k,Y_k 在 (1) 的状态转移下的形式。只需要维护 x,P

在卡尔曼滤波中的形式为：

hat x^{(+)}_k=hat x_{k}^{(-)}+P_{k}^{(+)}h^T_kr_k(y_k-h_kx_k^{(-)}) ag{8}

结合 (5),(6),(7) 共同组成了卡尔曼滤波的基本形式。其实就是最小二乘法

到这里，唯一与我们看到的卡尔曼滤波那几个公式形式不同的是 P_k的计算。下面给出递推计算 P_k .

有时候，为了方便计算，会定义： K_k=P_{k}^{(+)}h^T_kr_k .

出现了 经典的五个公式的影子。

递推计算 P_k

一个精巧的矩阵反演：

任意给定两个可逆矩阵 A,D

left[egin{matrix}A&B\ C&Dend{matrix} ight] ag{9}

令： E=D-CA^-B,F=A-BD^-C ,计算可得：

left[egin{matrix}A&B\ C&Dend{matrix} ight] left[egin{matrix}A^-+A^-BE^-CA^-&-A^-BE^-\ -E^-CA^-&E^-end{matrix} ight] =I ag{10}

left[egin{matrix}A&B\ C&Dend{matrix} ight] left[egin{matrix}F^-&-A^-BE^-\ -E^-CA^-&E^-end{matrix} ight] =I ag{11}

这也就是说，只要矩阵 A，D 可逆： (A+BD^-C)^-=A^--A^-B(D+CA^-B)^-CA^- ag{12}

那么递推计算 P_k=(H_k^TR_kH_k)^-=(P_{k-1}^-+h_k^Tr_kh_k)^-

根据 (12)

P_k=P_{k-1}-frac{P_{k-1}h^T_kh_kP_{k-1}}{r_k^-+h_kP_kh^T_k}=Big(I-frac{P_{k-1}h^T_kh_k}{r_k^-+h_kP_kh^T_k}Big)P_{k-1} ag{13}

令：

K_k=P_kh^T_kr_k =frac{P_{k-1}h^T_k}{r_k^-+h_kP_{k-1}h^T_k} ag{14}

则：

P_k=(I-K_kh_{k})P_{k-1} ag{15}

卡尔曼滤波更新公式总结

如此，卡尔曼滤波的基本形式也已经走通。本质上还是最小二乘法。只是在状态转移时传递了协方差。那么最小二乘法的几种形式均可以引用在卡尔曼滤波。例如设置遗忘。

hat x_{k}^{(-)}=F_{k-1}hat x_{k-1}^{(+)}+G_{k-1}u_{k-1} ag{16}

P_{k}^{(-)}=F_{k-1}P_{k-1}^{(+)}F_{k-1}^T+Q_{k-1} ag{17}

K_{k}=frac{P_{k}^{(-)}h_{k}^T}{r_k^-+h_kP_{k}^{(-)}h^T_k} ag{18}

P_{k}^{(+)}=(I-K_kh_k)P_{k}^{(-)} ag{19}

hat x_k^{(+)}=hat x_{k}^{(-)}+K_k(y_k-h_khat x_{k}^{(-)}) ag{20}

关于这5个公式：

回到最初 A,B 的那个问题。当我们知道k-1 时刻的 hat x_{k-1} 。到了 k 时刻，产生了一组观测数据： h_k,y_k .但由于 A 的速度一直在改变。那么我们要推算以现在的状态 (A) 观察历史的数据 (k-1时刻) 应该是什么样的。比如 A 在 B 的反向上做了加速。那么观察到的 B 的加速度应该更大了。通过 (16) 得出。同时，我们将协方差矩阵 P_{k-1} 通过 (17) 也传递过来。

然后通过 (18),(19),(20), 的递推最小二乘法计算当前 A 中观察下的 B 的状态。

如么没有 (1) 的状转移，那么我们其实也得出了递推版本的最小二乘法 (RLS)

K_{k}=frac{P_{k-1}^{}h_{k}^T}{r_k^-+h_kP_{k-1}^{}h^T_k} ag{21}

P_{k}^{}=(I-K_kh_k)P_{k-1}^{} ag{22}

hat x_k^{}=hat x_{k-1}^{}+K_k(y_k-h_khat x_{k-1}^{}) ag{23}

设置初始值

P_0 如何确定呢? .

根据最小二乘法：

P_k=Big(H_k^TR_kH_kBig)^-=Big(P_{k-1}^-+h_{k}^{T}r_kh_kBig)^-

得到：

P_k=Big(sum_{i=1}^{k}h_i^Tr_ih_iBig)^-approxBig(sum_{i=1}^{k}h_i^Tr_ih_i+ eta IBig )^-， eta ightarrow0

其中， eta 足够小。可以定义： P_0=eta^-I, eta^- ightarrowinfty

即： P_0=infty I

这是不是就在说明，当我们没有任何观测数据时， hat x 的方差为 infty


推荐阅读：
相关文章