Kalman滤波的扩展——渐消Fading filter滤波python实现_渐消因子-程序员宅基地

技术标签: Kalman  python  

原理:

在程序设计中,渐消因子的初值设为1。即:和1.7的表达式相同。

渐消滤波算例分析:

  1. 常加速度模型(一维)

历元间隔1s;状态向量维度为3,分别是位置、速度、加速度;每个历元观测向量维度为2,分别是位置和速度。

滤波初值设置:

位置

0m

速度

5m/s

加速度

0m/s2

状态向量

[0,5,0]

状态转移矩阵

[[1,dt,0.5dt2],[0,1,dt],[0,0,1]]

设计矩阵

[[1,0,0], [0,1,0]]

状态向量协方差阵

单位阵(3*3)

观测噪声协方差阵

[[100,0], [0,0.01]]

过程噪声矩阵

[[dt5/20, dt4/8, dt3/6],

[dt4/8, dt3/3, dt2/2],

[dt3/6, dt2/2, dt]]

假设载体在X轴方向运动,0-200s载体以5m/s的速度匀速运动,200-400s载体以0.05m/s2匀加速运动,400-600s以-0.02 m/s2匀减速运动,600-800s匀速运动,800-1000s停止状态。直接在轨迹发生器的位置和速度输出上叠加白噪声信号来模拟X轴方向上的位置和速度。

标准Kalman滤波

渐消Kalman滤波

在动力学模型误差较小的情况下,标准Kalman滤波比渐消Kalman要好很多。如果错误地将动力学模型建立错误,会是怎样地结果?

假设动力学模型中状态转移矩阵变为:

[[1,dt,0.5dt2],[0,3,dt],[0,0,1]]

标准Kalman滤波(错误动力学模型)

渐消Kalman滤波(错误动力学模型)

可以看到,采用渐消滤波位置不发散了,位置误差在25m以内。

  1. 假设载体进行匀加速运动,但设计动力学模型时误以为载体处于静止状态。

历元间隔1s;状态向量维度为1,位置;每个历元观测向量维度为1,位置。

滤波初值设置:

位置

0m

状态转移矩阵

[1]

设计矩阵

[1]

状态向量协方差阵

单位阵(1*1)

观测噪声协方差阵

[100]

过程噪声矩阵

[0.01]

0-400s载体以0.05m/s2进行匀加速运动。

标准Kalman滤波

渐消Kalman滤波

采用渐消Kalman滤波,误差大部分历元在20m以内,当动力学模型不准确的时候,采用渐消滤波可以有效抑制发散。

Python代码:

class FadingKalmanFilter(object):
    def __init__(self, alpha, dim_x, dim_z, dim_u=0):

        assert alpha >= 1
        assert dim_x > 0
        assert dim_z > 0
        assert dim_u >= 0

        self.alpha_ = alpha  # 渐消因子
        self.dim_x = dim_x  # 状态值个数
        self.dim_z = dim_z  # 观测值个数
        self.dim_u = dim_u  # 控制值个数

        self.x = zeros((dim_x, 1))  # Current state estimate 当前历元状态参数向量(dim_x*1)
        self.P = eye(dim_x)  # Current state covariance matrix 当前状态参数向量协方差阵(dim_x*dim_x)
        self.Q = eye(dim_x)  # Process noise covariance matrix 过程噪声协方差阵(dim_x*dim_x)
        self.B = 0.  # control transition matrix 控制矩阵(一般不考虑)
        self.F = np.eye(dim_x)  # state transition matrix  状态转移矩阵(dim_x*dim_x)
        self.H = zeros((dim_z, dim_x))  # Measurement function   观测设计矩阵(dim_z*dim_x)
        self.R = eye(dim_z)  # Measurement noise covariance matrix 观测噪声协方差矩阵(dim_z*dim_z)
        self.z = np.array([[None] * dim_z]).T

        # gain and residual are computed during the innovation step. We
        # save them so that in case you want to inspect them for various
        # purposes
        self.K = 0  # kalman gain 增益矩阵
        self.y = zeros((dim_z, 1))  # residual are computed during the innovation step 新息向量
        self.S = np.zeros((dim_z, dim_z))  # system uncertainty (measurement space)
        self.SI = np.zeros((dim_z, dim_z))  # inverse system uncertainty

        #self._alpha_sq = 1.  # fading memory control
        # identity matrix. Do not alter this.
        self._I = np.eye(dim_x)

        # Only computed only if requested via property 似然函数
        self._log_likelihood = log(sys.float_info.min)
        self._likelihood = sys.float_info.min
        self._mahalanobis = None

        # these will always be a copy of x,P after predict() is called
        self.x_prior = self.x.copy()  # 当前状态向量预测值
        self.P_prior = self.P.copy()  # 当前状态向量预测协方差阵

        # these will always be a copy of x,P after update() is called
        self.x_post = self.x.copy()  # 当前状态向量估值
        self.P_post = self.P.copy()  # 当前状态向量估值的协方差阵

        self.inv = np.linalg.inv

    def settingalpha_(self,z, u=None, B=None, F=None, Q=None,H=None,P=None,R=None):
        #计算预测状态向量
        if B is None:
            B = self.B
        if P is None:
            P = self.P
        if R is None:
            R = self.R
        if H is None:
            H = self.H
        if F is None:
            F = self.F
        if Q is None:
            Q = self.Q
        elif isscalar(Q):  # 如果Q是标量
            Q = eye(self.dim_x) * Q

        # x = Fx + Bu
        if B is not None and u is not None:
            x = dot(F, self.x) + dot(B, u)
        else:
            x = dot(F, self.x)
        #计算预测残差向量
        y= z - dot(H, self.x)
        #计算预测残差向量的协方差阵
        S=self.alpha_*dot(y,y.T)/(1+self.alpha_)

        HF=dot(H,F)
        HFP=dot(HF,P)
        M=dot(HFP,HF.T)
        HQ=dot(H,Q)
        N=S-dot(HQ,H.T)-R


        alpha=np.trace(N)/np.trace(M)

        if alpha>=1:
            self.alpha_=alpha
        else:
            self.alpha_=1.




        pass
    # 预测
    def predict(self, u=None, B=None, F=None, Q=None):
        """
        Predict next state (prior) using the Kalman filter state propagation
        equations.
        Parameters
        ----------
        u : np.array, default 0
            Optional control vector.控制向量(一般不用)

        B : np.array(dim_x, dim_u), or None
            Optional control transition matrix; a value of None
            will cause the filter to use `self.B`.

        F : np.array(dim_x, dim_x), or None
            Optional state transition matrix; a value of None
            will cause the filter to use `self.F`.

        Q : np.array(dim_x, dim_x), scalar, or None
            Optional process noise matrix; a value of None will cause the
            filter to use `self.Q`.
        """

        if B is None:
            B = self.B
        if F is None:
            F = self.F
        if Q is None:
            Q = self.Q
        elif isscalar(Q):  # 如果Q是标量
            Q = eye(self.dim_x) * Q

        # x = Fx + Bu
        if B is not None and u is not None:
            self.x = dot(F, self.x) + dot(B, u)
        else:
            self.x = dot(F, self.x)

        # P = FPF' + Q P变为状态预测向量的协方差矩阵
        self.P = self.alpha_ * dot(dot(F, self.P), F.T) + Q

        # save prior
        self.x_prior = self.x.copy()
        self.P_prior = self.P.copy()

    def update(self, z, R=None, H=None):
        """
        Add a new measurement (z) to the Kalman filter.
        If z is None, nothing is computed. However, x_post and P_post are
        updated with the prior (x_prior, P_prior), and self.z is set to None.
        Parameters
        ----------
        z : (dim_z, 1): array_like 当前历元观测向量
            measurement for this update. z can be a scalar if dim_z is 1,
            otherwise it must be convertible to a column vector.
            If you pass in a value of H, z must be a column vector the
            of the correct size.
        R : np.array, scalar, or None 测量噪声矩阵
            Optionally provide R to override the measurement noise for this
            one call, otherwise  self.R will be used.
        H : np.array, or None  设计矩阵
            Optionally provide H to override the measurement function for this
            one call, otherwise self.H will be used.
        """

        # set to None to force recompute
        self._log_likelihood = None
        self._likelihood = None
        self._mahalanobis = None

        if z is None:
            self.z = np.array([[None] * self.dim_z]).T
            self.x_post = self.x.copy()
            self.P_post = self.P.copy()
            self.y = zeros((self.dim_z, 1))
            return

        if R is None:
            R = self.R
        elif isscalar(R):
            R = eye(self.dim_z) * R

        if H is None:
            # z = reshape_z(z, self.dim_z, self.x.ndim)
            H = self.H

        # y = z - Hx 新息向量y
        # error (residual) between measurement and prediction
        self.y = z - dot(H, self.x)

        # common subexpression for speed
        PHT = dot(self.P, H.T)

        # S = HPH' + R
        # project system uncertainty into measurement space
        # S 新息向量y的协方差矩阵 R观测向量噪声的协方差矩阵
        self.S = dot(H, PHT) + R
        self.SI = self.inv(self.S)
        # K = PH'inv(S)
        # map system uncertainty into kalman gain
        # K 增益矩阵
        self.K = dot(PHT, self.SI)

        # x = x + Ky
        # 当前历元的状态向量估值
        # predict new x with residual scaled by the kalman gain
        self.x = self.x + dot(self.K, self.y)

        # P = (I-KH)P(I-KH)' + KRK'
        # This is more numerically stable
        # and works for non-optimal K vs the equation
        # P = (I-KH)P usually seen in the literature.
        # 状态向量估值的协方差矩阵
        I_KH = self._I - dot(self.K, H)
        self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T)

        # save measurement and posterior state
        self.z = deepcopy(z)
        self.x_post = self.x.copy()
        self.P_post = self.P.copy()

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/weixin_43794896/article/details/118695826

智能推荐

CSS box-flex属性-程序员宅基地

文章浏览阅读52次。box-flex属性(和谐版)#father { display: box; }#first_boy { box-flex: 2; }#second_boy{ box-flex: 1; }#three_boy { box-flex: 1; }CSS box-flex属性(不和谐版)#first_boy { box-flex: 2; }#second_boy {...

NSJSONSerialization_ios nsjsonserialization da-程序员宅基地

文章浏览阅读492次。介绍: 在了解NSJSONSerialization之前我们需要知道JSON这个东西,JSON是什么呢!是一种轻量级的数据交换格式,更可以 理解为后台服务器传来的奇怪数据,然而NSJSONSerialization就是可以解开这个奇怪数据的方法,其实解开的方法有 很多,如:TouchJSON、SBJSON、JSONKit等等,但是NSJSONSerialization是苹果自家开发,所以性_ios nsjsonserialization da

公钥,私钥,证书_数字证书 bob alice-程序员宅基地

文章浏览阅读357次。Bob,Alice和数字证书网络安全中最知名的人物大概就是Bob和Alice了,因为很多安全原理阐述中都用这两个虚拟人物来进行实例说明。我们来看看Bob是怎么从CA中心获得一个数字证书的:1、Bob首先创建他自己的密钥对(key pair),包含公钥和私钥;2、Bob通过网络把他的公钥送到CA中心,公钥中包含了Bob的个人鉴别信息(他的名字、地址、所用设备的序列号等等)_数字证书 bob alice

基于JAVA便行顺风车出行系统计算机毕业设计源码+系统+lw文档+部署_顺风车辅助 csdn-程序员宅基地

文章浏览阅读119次。基于JAVA便行顺风车出行系统计算机毕业设计源码+系统+lw文档+部署。springboot基于SpringBoot的商城后台管理系统。jsp基于JSP的天津城建大学计算机学院校友录管理系统。ssm基于VUE.js的保护环境的App的开发与实现。ssm基于Vue的潍坊学院宿舍管理系统的设计与实现。JSP宠物食品店系统的设计与实现sqlserver。ssm基于HTML的“守护萌宠”网站的设计与实现。ssm基于SSM的线上家庭医生系统的设计与实现。ssm基于B_S景区票务管理系统设计与实现。_顺风车辅助 csdn

immersionBar 设置颜色报错: android.content.res.Resources$NotFoundException: Resource ID #0xffff0000_immersionbar 使用自定义颜色报错-程序员宅基地

文章浏览阅读135次。直接传 R.color.blackgetResources().getColor(R.color.black) 和 Color.BLACK 都是错的 会报错_immersionbar 使用自定义颜色报错

qt--防止子窗口重复_qt 如何避免打开多个相同窗口-程序员宅基地

文章浏览阅读3.4k次。1、窗口定为模态的,从根本上解决问题,但是无法操作另外窗口。若不关掉此窗口,则无法操作。 使用非模态窗口,可以解决此问题,但是非模态窗口可以重复出现窗口。2、解决重复出现窗口问题: 先创建一个空指针,在创建窗体的时候判断该指针是否为NULL,如果为NULL,表示窗体未创建,则创建窗体;如果不为NULL,则表示已经创建窗体,则不再创建新窗体。用null作为判断的条件,会默认返回false// class A;// class B;void A::openB() { ..._qt 如何避免打开多个相同窗口

随便推点

LaTeX排版学习记录_texstudio中大括号怎么输入-程序员宅基地

文章浏览阅读572次。从2016年02月21开始,逐步学习LaTeX在科技文献中的使用,该文章作为学习记录笔记。% start.tex\documentclass[utf8]{article}\begin{document}This is the start!\end{document}_texstudio中大括号怎么输入

Android国家区号 中英文_flutter 获取第三方的国际区号列表-程序员宅基地

文章浏览阅读4.5k次。<array name="country_code_list"> <item>阿富汗 *+93</item> <item>阿尔巴尼亚 *+355</item> <item>阿尔及利亚 *+213</item> <_flutter 获取第三方的国际区号列表

Spring SpringMVC SpringBoot SpringCloud概念及关系_spring,springmvc,springcloud和springboot分别指什么有什么关联-程序员宅基地

文章浏览阅读264次。一、Spring SpringMVC SpringBoot SpringCloud概念、关系及区别Spring主要是基于IOC反转Beans管理Bean类,主要依存于SSH框架(Struts+Spring+Hibernate)这个MVC框架,所以定位很明确,Struts主要负责表示层的显示,Spring利用它的IOC和AOP来处理控制业务(负责对数据库的操作),Hibernate主要作用是数据的持久化到数据库。SpringMVC是基于Spring的一个MVC框架,用以替代初期的SSH框架;(spring_spring,springmvc,springcloud和springboot分别指什么有什么关联

Unity5.x学习笔记(2)-物体向上飞_unity物体向上飞-程序员宅基地

文章浏览阅读2.8k次。在学习克隆游戏对象这一节时,例子是创建一个带刚体组件的预制体,然后用脚本复制它,结果运行后发现物体向上飞了。后来发现原来是主摄像机不小心也添加了刚体组件,而场景中原本有一个不带刚体的cube1,新复制出来的带刚体的cube2和cube1碰撞了一下,斜飞了出去,而摄像机垂直掉落,导致看起来好像是两个cube一个向上飞,一个向斜上飞了。其实是一个没动,一个向斜下飞。_unity物体向上飞

谈抽象类与接口的区别_1.抽象类与接口的区别?-程序员宅基地

文章浏览阅读391次。一、抽象类:抽象类是特殊的类,只是不能被实例化;除此以外,具有类的其他特性;重要的是抽象类可以包括抽象方法,这是普通类所不能的。抽象方法只能声明于抽象类中,且不包含任何实现,派生类必须覆盖它们。另外,抽象类可以派生自一个抽象类,可以覆盖基类的抽象方法也可以不覆盖,如果不覆盖,则其派生类必须覆盖它们。二、接口:接口是引用类型的,类似于类,和抽象类的相似之处有三点:1、不能实例化;_1.抽象类与接口的区别?

SpringCloud微服务开发,优先访问本地服务的配置_prior-ip-pattern-程序员宅基地

文章浏览阅读4.1k次,点赞2次,收藏7次。本地开发起了一个微服务,优先访问本地服务。需要在本地启动一个网关应用,在网关配置文件配置如下:blade: #多团队协作服务配置 ribbon: rule: #开启配置 enabled: true #负载均衡优先调用的ip段 prior-ip-pattern: - 192.168.8.6 - 127.0.0.1有限访问本地的ip地址前端vue项目配置:配置前端服务访问本地网关.._prior-ip-pattern