第11章TheExtendedKalmanFilter-创新互联

第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

目前成都创新互联已为上1000家的企业提供了网站建设、域名、虚拟主机、网站托管、服务器托管、企业网站设计、昌都网站维护等服务，公司将坚持客户导向、应用为本的策略，正道将秉承"和谐、参与、激情"的文化，与客户和合作伙伴齐心协力一起成长，共同发展。

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=∂x∂f(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=z−HxˉK=PˉHT(HPˉHT+R)−1x=xˉ+KyP=(I−KH)PˉEKFF=∂x∂f(xt,ut)∣∣∣∣xt,utxˉ=f(x,u)Pˉ=FPFT+QH=∂xˉ∂h(xˉt)∣∣∣∣xˉty=z−h(xˉ)K=PˉHT(HPˉHT+R)−1x=xˉ+KyP=(I−KH)Pˉ

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 = ( 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⎦⎤=⎣⎡000100000⎦⎤⎣⎡x0x1x2⎦⎤+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=eAAAdt≈III+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)=⎣⎡100dt10001⎦⎤xxx(k−1)

测量方程
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

这里有几点需要注意：

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

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

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

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

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

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

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

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

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

在这里插入图片描述

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

在这里插入图片描述

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

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