zoukankan      html  css  js  c++  java
  • 卡尔曼滤波 (Kalman Filter)的一个简单实现: 恒定加速度模型

    恒定加速度Kalman模型


    1. scenario:我们想用卡尔曼滤波来追踪3D空间中某个具有恒定加速度的物体,我们有一个位置传感器可以用来观测物体位置,我们想得到物体的3D位置以及速度。


    2. Description :


    假设物体状态由六维vector定义如下:

     









     

    系统的预测方程为:

     


     

    假设我们没有控制输入,那么预测方程可以表示如下:

    y = Hx


    因为我们有位置传感器,不管是视觉,激光还是雷达,我们可以得到物体在3D空间中的位置,即(xyz),那么系统的观测方程可以用如下表示:






    接下来我们定义初始状态的协方差矩阵为9x9的单位矩:

     








    接下来定义系统的状态转移矩阵A,若定义状态间的time stepdt = 1,则此时的A矩阵为:








     

    定义系统的初始观测协方差矩阵为3x3的对角阵:





    :系统的加速度扰动 ,那么Q可以用如下表示:




     

    3. Implementation

     

    这里省略了一些细节,具体代码请查看我的github主页:https://github.com/Doodleshr

     

     

    
    P = 100.0*np.eye(9)
    
    #define dt = 0.05 rather than dt = 1
    dt = 0.05
    
    #define H and A according to what we have discused
    ......
    ###
    #define observation noise covariance R = 100*np.eye(3) #define process noise matrix Q #from the blog we mentioned how to compute Q from G G = ... a = 0.1 Q = G*G.T*a**2
    #system initialization px
    =0.0 py=0.0 pz=0.0 vx=1 vy=2 vz=3 Xr=[] Yr=[] Zr=[] # #generate position: for i in range(100): # we assume constant acceleratoin for this demo accx = 3 vx += accx * dt px += vx * dt accy = 1 vy += accy * dt py += vy * dt accz = 1 vz += accz * dt pz += vz * dt Xr.append(px) Yr.append(py) Zr.append(pz) # position noise sp = 0.5 Xm = Xr + sp * (np.random.randn(100)) Ym = Yr + sp * (np.random.randn(100)) Zm = Zr + sp * (np.random.randn(100)) #stack measurements measurements = np.vstack((Xm,Ym,Zm)) #define initial state X = np.matrix([0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 3.0, 1.0, 1.0]).T I = np.eye(9) Xx = [] Xy = [] Xz = [] for each_step in range(100): #prediction step: X = A*X #state covariance propogation P = A*P*A.T + Q #measurement covariance propogation S = H*P*H.T + R #Kalman Gain update K = (P*H.T) * np.linalg.pinv(S) # print(K) Z = measurements[:,each_step].reshape(3,1) #corrected/updated x X = X + K*(Z-(H*X)) #update state covariance P: P = (I - (K * H)) * P #store corrected/updated x Xx.append(float(X[0])) Xy.append(float(X[1])) Xz.append(float(X[2]))

     

     

    4. Result:

     



    我们可以看到观测值是很noisy的,但是使用卡尔曼滤波后,轨迹变得平滑许多,也更加贴合实际轨迹,可见卡尔曼滤波是是有效的。

  • 相关阅读:
    NO17--vue父子组件间单向数据流的解决办法
    NO--16 vue之父子组件传值
    使用Element-ui开发桌面应用的小问题
    微信小程序右到左联动
    微信小程序左到右联动
    微信小程序选项卡之跳转
    微信小程序
    回调函数和钩子函数的说明
    最近项目中遇到了一个场景,其实很常见,就是定时获取接口刷新数据。那么问题来了,假设我设置的定时时间为1s,而数据接口返回大于1s,应该用同步阻塞还是异步?
    组件的相互嵌套slot内容分发
  • 原文地址:https://www.cnblogs.com/SongHaoran/p/7587527.html
Copyright © 2011-2022 走看看