第11章 The Extended Kalman Filter

第11章 The Extended Kalman Filter

11.1 线性化卡尔曼滤波

  • Linear Kalman Filter, 状态方程

x ˙ = A x + B u + w z = H x + v \begin{aligned} \dot{\pmb{x}} &= \pmb{Ax}+\pmb{Bu}+\pmb{w} \\ \pmb{z} &= \pmb{Hx}+\pmb{v} \end{aligned} xxx˙zzz=AxAxAx+BuBuBu+www=HxHxHx+vvv

  • Extended Kalman Filter, 状态方程

x = f ( x , u ) + w z = h ( x ) + ν \begin{aligned} \pmb{x} &= \pmb{f}(\pmb{x},\pmb{u})+\pmb{w}\\ \pmb{z} &= \pmb{h}(\pmb{x})+\pmb{\nu} \end{aligned} xxxzzz=fff(xxx,uuu)+www=hhh(xxx)+ννν

F = ∂ f ( x t , u t ) ∂ x ∣ x t , u t H = ∂ h ( x ˉ t ) ∂ x ˉ ∣ x ˉ t \begin{aligned} \mathbf F &= {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}} \\ \mathbf H &= \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t} \end{aligned} FH=xf(xt,ut)xt,ut=xˉh(xˉt)xˉt

linear Kalman filter EKF F = ∂ f ( x t , u t ) ∂ x ∣ x t , u t x ˉ = F x + B u x ˉ = f ( x , u ) P ˉ = F P F T + Q P ˉ = F P F T + Q H = ∂ h ( x ˉ t ) ∂ x ˉ ∣ x ˉ t y = z − H x ˉ y = z − h ( x ˉ ) K = P ˉ H T ( H P ˉ H T + R ) − 1 K = P ˉ H T ( H P ˉ H T + R ) − 1 x = x ˉ + K y x = x ˉ + K y P = ( I − K H ) P ˉ P = ( I − K H ) P ˉ \begin{array}{l|l} \text{linear Kalman filter} & \text{EKF} \\ \hline & \boxed{\mathbf F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}}} \\ \mathbf{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)} \\ \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\ \hline & \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\ \textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\ \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\ \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\ \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} \end{array} linear Kalman filterxˉ=Fx+BuPˉ=FPFT+Qy=zHxˉK=PˉHT(HPˉHT+R)1x=xˉ+KyP=(IKH)PˉEKFF=xf(xt,ut)xt,utxˉ=f(x,u)Pˉ=FPFT+QH=xˉh(xˉt)xˉty=zh(xˉ)K=PˉHT(HPˉHT+R)1x=xˉ+KyP=(IKH)Pˉ

11.2 实例

第11章 The Extended Kalman Filter_第1张图片
背景: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 = ( x 0 x 1 x 2 ) T \pmb{x} = \begin{pmatrix} x_0 & x_1 & x_2 \end{pmatrix}^T xxx=(x0x1x2)T

x 0 {x}_0 x0:水平距离。 x 1 {x}_1 x1:水平速度。 x 2 {x}_2 x2:垂直高度。

  • 系统状态方程:(需要注意,速度只有水平方向,所以 x ˙ 0 = x 1 \dot{x}_0=x_1 x˙0=x1

[ x ˙ 0 x ˙ 1 x ˙ 2 ] = [ 0 1 0 0 0 0 0 0 0 ] [ x 0 x 1 x 2 ] + w \begin{bmatrix} \dot{x}_0 \\ \dot{x}_1 \\ \dot{x}_2 \\ \end{bmatrix} =\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} x_0 \\ x_1 \\ x_2 \\ \end{bmatrix} + \pmb{w} x˙0x˙1x˙2=000100000x0x1x2+www

系统的状态方程是连续的线性的,将其离散化(对确定性部分):

F = e A d t ≈ I + A d t \begin{aligned} \pmb{F} &= e^{\pmb{A}dt} \\ &\approx \pmb{I} + \pmb{A}dt \end{aligned} FFF=eAAAdtIII+AAAdt

所以离散化后的状态方程有:(其实这里还有一个问题, w \pmb{w} www变成什么了?)

x ( k ) = [ 1 d t 0 0 1 0 0 0 1 ] x ( k − 1 ) \pmb{x}(k)= \begin{bmatrix} 1 & dt & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \pmb{x}(k-1) xxx(k)=100dt10001xxx(k1)

  • 测量方程
    z = h ( x ) + v = x 0 2 + x 2 2 + v \begin{aligned} \pmb{z} &= \pmb{h}(\pmb{x}) + \pmb{v} \\ &= \sqrt{x_{0}^{2}+x_{2}^{2}} + \pmb{v} \\ \end{aligned} zzz=hhh(xxx)+vvv=x02+x22 +vvv
    离散化后,测量方程为;
    z k = [ x 0 x 0 2 + x 2 2 0 x 2 x 0 2 + x 2 2 ] x k \pmb{z}_k= \begin{bmatrix} \frac{x_0}{x_0^2+x_2^2} & 0 & \frac{x_2}{x_0^2+x_2^2} \end{bmatrix} \pmb{x}_k zzzk=[x02+x22x00x02+x22x2]xxxk

这里有几点需要注意:

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

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

  • 仿真结果(及其调试的过程记录)
    • 协方差矩阵的更新公式,在代码中的实现出现了错误。/_ \
      在这里插入图片描述
      第11章 The Extended Kalman Filter_第2张图片
      第11章 The Extended Kalman Filter_第3张图片
      第11章 The Extended Kalman Filter_第4张图片
      第11章 The Extended Kalman Filter_第5张图片
    • 然后改到正确的公式后,它竟然还是发散的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还有数组和矩阵的区别,我是真的醉醉的呢啊,下面我该怎么办?(就用数组类型了,现在就是要搞懂数组的运算,当然刚才维数这里也搞错了一点问题,继续加深理解呗)

第11章 The Extended Kalman Filter_第6张图片

第11章 The Extended Kalman Filter_第7张图片
第11章 The Extended Kalman Filter_第8张图片

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

第11章 The Extended Kalman Filter_第9张图片
第11章 The Extended Kalman Filter_第10张图片
第11章 The Extended Kalman Filter_第11张图片
现在这里还有一个很明显的问题,速度的收敛速度好像有点慢,而且最后对于高度的估计有发散的趋势吗?(延长一点时间看看?)
然后它它,就直接起飞!(分析原因,因为随着距离的增加,radar的噪声逐渐增大的,然后就不能正确的估计啦?后面调试后确实是的)
第11章 The Extended Kalman Filter_第12张图片
第11章 The Extended Kalman Filter_第13张图片
第11章 The Extended Kalman Filter_第14张图片
一个这个程序,竟然调了两天,我???

分析原因:

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

参考资料:

Kalman-and-Bayesian-Filters-in-Python

你可能感兴趣的:(Kalman,and,Bayesian,Filters,算法,python)