RLS算法到卡尔曼滤波 II

接着上一篇文章：https://blog.csdn.net/ZLH_HHHH/article/details/90515377

卡尔曼滤波在原本的$RLS$基础上增加了一个线性系统。
卡尔曼滤波应用于下面的系统：
$$x(k)=F(k-1)x(k-1)+G(k-1)u(k-1)+w(k-1)\text{\hspace{1em}(1)}$$
$$y(k)=h(k)x(k)+v(k)\text{\hspace{1em}(2)}$$

其中$w,v$是零均值,不相关白噪音。即协方差为$0$.
这里，$x$ 就是当前系统的状态，比如速度，位置，温度，等等。
$(1)$式可以理解为，上一个时刻，系统的一个输入，对系统产生的影响。系统的状态改变。比如突然有外力做工，从新更新状态。但是这个更新是有误差的。
$(2)$式子，可以认为是测量，$h(k)$对应的在$x(k)$状态下的输出$y(k)$被我们测量出来，有一个不可观测的随机误差$v(k)$，$h,y$都是已知的。

经过$(1)$的计算，对$x$状态进行了一次转移。并被$(2)$式纠正。

动态的系统，之前的$RLS$已经不再适用,$x(k-1)$的状态在不停改变的。或者说需要修改$RLS$算法。

$RLS$算法关键的一点就是：
$$P(k)=(H^T(k)H(k){)}^{-}=E((\hat{x}(k)-x(k))(\hat{x}(k)-x(k){)}^T)$$
即，$P(k)$是估计$\hat{x}$的协方差。而协防差在$(1)$式状态改变的过程中也被传递了。
这里，由于误差是无偏的，一般认为$E(\hat{x})=x$
这也就是说：
$$\begin{gathered} 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)=E(\hat{x}(k)) \end{gathered}$$

则：$E(\hat{x}(k)-x(k))=F(k-1)E(\hat{x}(k-1)-x(k-1))+E(w(k-1))$

那么我们可以根据$k-1$时刻以及此时状态转移来估计新的协方差：
$$\begin{gathered} P(k)=E((\hat{x}(k)-x(k)(\hat{x}(k)-x(k{)}^T) \\ =E([F(k-1)(\hat{x}(k-1)-x(k-1))+w(k-1)][F(k-1)(\hat{x}(k-1)-x(k-1))+w(k-1){]}^T) \\ =F(k-1)P(k-1)F^T(k-1)+Q(k-1) \end{gathered}$$
其中:$Q(k)=E(w(k)w^T(k))$

当我们没有新的观测数据时，利用旧的观测来做一个先验估计 $x(k{)}^{-}$
而通过新的观测纠正后的数据为后验：$x(k{)}^{+}$
结合$RLS$, 得出卡尔曼滤波：
$$\begin{gathered} x(k{)}^{-}=F(k-1)x(k-1{)}^{+}+G(k-1)u(k-1) \\ P(k{)}^{-}=F(k-1)P(k-1{)}^{+}{F}^T(k-1)+Q(k-1) \end{gathered}$$
$$K(k)=\frac{P(k{)}^{-}{h}^T(k)}{R(k)+h(k)P(k{)}^{-}{h}^T(k)}\text{\hspace{1em}(RLS)}$$
$$P(k{)}^{+}=(I-K(k)h(k))P(k{)}^{-}\text{\hspace{1em}(RLS)}$$
$$\hat{x}(k{)}^{+}=\hat{x}(k{)}^{-}+K(k)(y(k)-h(k)\hat{x}(k{)}^{-})\text{\hspace{1em}(RLS)}$$

卡尔曼滤波其实就是通过系统状态转移，跟踪了协方差的状态转移。然后从新带入到$RLS$更新中.

最关键的还是$P(k)$的属性:
$$P(k)=(H^T(k)H(k){)}^{-}=E((\hat{x}-x)(\hat{x}-x{)}^T)$$
$P$的这一属性使得参考系改变时得以根据状态转移矩阵同时被转移。
怎么说呢， 比如，两架飞机，$A,B$,当$A$匀加速速运动时，观察到匀加速运动的$B$。
当$A$在处理上一时刻状态时，其实自身的状态，也改变了，速度啊，速率，位置什么的，都改变。这个时候使用$(1)$式，进行状态转移，由于$P$的关键属性,即使$H^{T}H$的逆，又是$x$的协方差。如此甚好。对$P$进行一次状态转移。这次转移，是为了使用新的观测数据。来调用$RLS$的三段更新。

到这里，对卡尔曼滤波有了一个浅显的认识，嘿嘿。