第11章 The Extended Kalman Filter

第11章 The Extended Kalman Filter

11.1 线性化卡尔曼滤波

• Linear Kalman Filter, 状态方程

$$\begin{array}{rl}\dot{x\text{x}x}& =Ax\text{Ax}Ax+Bu\text{Bu}Bu+w\text{w}w\\ z\text{z}z& =Hx\text{Hx}Hx+v\text{v}v\end{array}$$

• Extended Kalman Filter, 状态方程

$$\begin{array}{rl}x\text{x}x& =f\text{f}f(x\text{x}x,u\text{u}u)+w\text{w}w\\ z\text{z}z& =h\text{h}h(x\text{x}x)+\nu \text{ν}\nu \end{array}$$

$$\begin{array}{rl}\mathbf{F}& =\frac{\partial f({\mathbf{x}}_t,{\mathbf{u}}_t)}{\partial \mathbf{x}}{|}_{{\mathbf{x}}_t,{\mathbf{u}}_t}\\ \mathbf{H}& =\frac{\partial h({\bar{\mathbf{x}}}_t)}{\partial \bar{\mathbf{x}}}{|}_{{\bar{\mathbf{x}}}_t}\end{array}$$

$$\begin{array}{ll}\text{linear Kalman filter}& \text{EKF}\\ & \boxed{\mathbf{F}=\frac{\partial f({\mathbf{x}}_t,{\mathbf{u}}_t)}{\partial \mathbf{x}}{|}_{{\mathbf{x}}_t,{\mathbf{u}}_t}}\\ \bar{\mathbf{x}}=\mathbf{F}\mathbf{x}+\mathbf{B}\mathbf{u}& \boxed{\bar{\mathbf{x}}=f(\mathbf{x},\mathbf{u})}\\ \bar{\mathbf{P}}={\mathbf{F}\mathbf{P}\mathbf{F}}^{\mathsf{T}}+\mathbf{Q}& \bar{\mathbf{P}}={\mathbf{F}\mathbf{P}\mathbf{F}}^{\mathsf{T}}+\mathbf{Q}\\ & \boxed{\mathbf{H}=\frac{\partial h({\bar{\mathbf{x}}}_t)}{\partial \bar{\mathbf{x}}}{|}_{{\bar{\mathbf{x}}}_t}}\\ \text{y}=\mathbf{z}-\mathbf{H}\bar{\mathbf{x}}& \text{y}=\mathbf{z}-\boxed{h(\bar{x})}\\ \mathbf{K}={\bar{\mathbf{P}}\mathbf{H}}^{\mathsf{T}}({\mathbf{H}\bar{\mathbf{P}}\mathbf{H}}^{\mathsf{T}}+\mathbf{R}{)}^{-1}& \mathbf{K}={\bar{\mathbf{P}}\mathbf{H}}^{\mathsf{T}}({\mathbf{H}\bar{\mathbf{P}}\mathbf{H}}^{\mathsf{T}}+\mathbf{R}{)}^{-1}\\ \mathbf{x}=\bar{\mathbf{x}}+\mathbf{K}\text{y}& \mathbf{x}=\bar{\mathbf{x}}+\mathbf{K}\text{y}\\ \mathbf{P}=(\mathbf{I}-\mathbf{K}\mathbf{H})\bar{\mathbf{P}}& \mathbf{P}=(\mathbf{I}-\mathbf{K}\mathbf{H})\bar{\mathbf{P}}\end{array}$$

11.2 实例

背景：1）考虑二维平面；2）Aircraft只有水平方向的速度；3）Aircraft高度假设不变；4）Radar随着距离的变大，精度减低

• 下面这个是Radar的模型：

class Radar:
    """模拟雷达传感器(二维平面)"""

    def __init__(self, dt, pos, vel, alt):
        """
        Parameters
        ----------
        dt : double
            采样时间
        pos : double
            水平距离
        vel : double
            水平速度
        alt : double
            垂直高度
        Returns
        -------
        None.
        """

        # 初始化Radar
        self.dt = dt
        self.pos = pos
        self.vel = vel
        self.alt = alt

    def get_range(self):
        """得到物体当前运动信息"""

        # 添加一些噪声
        self.vel = self.vel + .1*randn()
        self.alt = self.alt + .1*randn()
        # 从上一刻的位置获得当前时刻的位置
        self.pos = self.pos + self.vel*self.dt

        # 假设雷达的精度随着距离变远而越来越低
        err = self.pos * (0.05*randn())
        dist = math.sqrt(self.pos**2 + self.alt**2) + err

        return dist

下面说明系统的状态方程和测量方程，首先得确定系统的状态变量$x\text{x}x={\left(\begin{array}{ccc}{x}_0& x_1& x_2\end{array}\right)}^T$

$x_0$：水平距离。$x_1$：水平速度。$x_2$：垂直高度。

• 系统状态方程：（需要注意，速度只有水平方向，所以${\dot{x}}_0=x_1$）

$$\left[\begin{array}{c}{\dot{x}}_0\\ {\dot{x}}_1\\ {\dot{x}}_2\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_0\\ x_1\\ x_2\end{array}\right]+w\text{w}w$$

系统的状态方程是连续的线性的，将其离散化（对确定性部分）：

$$\begin{array}{rl}F\text{F}F& =e^{A\text{A}Adt}\\ & \approx I\text{I}I+A\text{A}Adt\end{array}$$

所以离散化后的状态方程有：(其实这里还有一个问题，$w\text{w}w$变成什么了？）

$$x\text{x}x(k)=\left[\begin{array}{ccc}1& dt& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]x\text{x}x(k-1)$$

• 测量方程
  $$\begin{array}{rl}z\text{z}z& =h\text{h}h(x\text{x}x)+v\text{v}v\\ & =\sqrt{x_0^2+x_2^2}+v\text{v}v\end{array}$$
  离散化后，测量方程为；
  $${z\text{z}z}_k=\left[\begin{array}{ccc}\frac{x_0}{x_0^2+x_2^2}& 0& \frac{x_2}{x_0^2+x_2^2}\end{array}\right]{x\text{x}x}_k$$

这里有几点需要注意：

1. 特别是随机噪声$Q\text{Q}Q,R\text{R}R$，需要根据实际的情况进行不断的调整
2. 初始值的设定具有很大的随机性，${x\text{x}x}_0,{P\text{P}P}_0$几乎没有什么影响。

关于这一点可以参考一篇文献，严恭敏老师的《传统组合导航中的实用Kalman滤波技术评述》，给出了很多实用的建议：1）只要滤波器是渐近稳定，初值不会影响估计的最终结果（和系统的可观性有一定的关系）；2）去精确的离散化噪声的协方差矩阵，还不如增加采样频率所提高的精度多。

• 仿真结果（及其调试的过程记录）
  • 协方差矩阵的更新公式，在代码中的实现出现了错误。/_ \
    
    
    
    
    
  • 然后改到正确的公式后，它竟然还是发散的X_X

问题分析：1）在初始值的估计值还是没有偏差的情况下进行的。。2）

解决办法：1）查看kalman滤波是否正确表达了；2）将初始值调为有一定偏差的，看是否有其它的问题；3)增加计算的时间后，又出现了NaN的数据，看来电脑的计算还是没有搞清楚！

结果：发现对于Numpy的数值的维度有点混乱，导致矩阵的计算出现了些许问题。现在已经有点晕了，需要重新再来一遍（遇到了一点问题，于是就又想逃避了哈？？还是先吃饭吧）

(python numpy) np.array.shape 中 (3,)、(3,1)、(1,3)的区别_我是一颗棒棒糖的博客-CSDN博客_np.array.shape

这里在强调一下，需要注意一下这个的运算过程，看看是否对于np.dot有什么误解呢。（突然好想用matlab啊，这个python是真的┭┮﹏┭┮）

numpy还有数组和矩阵的区别，我是真的醉醉的呢啊，下面我该怎么办？(就用数组类型了，现在就是要搞懂数组的运算，当然刚才维数这里也搞错了一点问题，继续加深理解呗）

• 经过仔细的调整，主要就是numpy的narray的运算进行了详细的对比，现在的结果稍微好了一点点呢！（这里对于状态的初始值也进行了调整和容易看出来的）

现在这里还有一个很明显的问题，速度的收敛速度好像有点慢，而且最后对于高度的估计有发散的趋势吗？（延长一点时间看看？）
然后它它，就直接起飞！（分析原因，因为随着距离的增加，radar的噪声逐渐增大的，然后就不能正确的估计啦？后面调试后确实是的）



一个这个程序，竟然调了两天，我？？？

分析原因：

1. EKF还是不太熟悉，连公式都打错
2. 第一次用Python来写，对于一些基本的运算不太了解，导致很多错误的都是计算的形式有问题
3. 遇到问题的心态要好好调整，哪里不会就点哪个（人生苦短，不用什么都要学透）

参考资料：

Kalman-and-Bayesian-Filters-in-Python