码迷,mamicode.com
首页 > 其他好文 > 详细

卡尔曼滤波的一个简单demo: 恒定加速度模型

时间:2017-09-24 16:48:07      阅读:280      评论:0      收藏:0      [点我收藏+]

标签:image   str   acm   vsx   veh   drm   bof   vfp   nps   

恒定加速度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的,但是使用卡尔曼滤波后,轨迹变得平滑许多,也更加贴合实际轨迹,可见卡尔曼滤波是是有效的。

卡尔曼滤波的一个简单demo: 恒定加速度模型

标签:image   str   acm   vsx   veh   drm   bof   vfp   nps   

原文地址:http://www.cnblogs.com/SongHaoran/p/7587527.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!