整体思路很简单,卡尔曼滤波器就是做数据融合的,先给一个GPS的数据(z)和一个里程计数据(u),让他们融合吧。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Dec 18 19:37:13 2018
@author: sc
args explanition:
p:covariance
x:state
z:observation
u:control
Pred:predict
Est:estatemation
Q:表示过程激励噪声的协方差,它是状态转移矩阵与实际过程之间的误差。
R:表示测量噪声协方差,和仪器相关的一个特性,
"""
import numpy as np
import math
import matplotlib.pyplot as plt
Q=np.diag([0.1,0.1,math.radians(1.0),1.0])**2
R=np.diag([1.0,math.radians(40.0)])**2
dt=0.1
def motion_model(x,u):
B=np.matrix([[dt*math.cos(x[2,0]),0.0],
[dt*math.sin(x[2,0]),0.0],
[0.0,dt],
[1.0,0.0]])
x=x+B*u
return x
#def observe_model(z):
# H=
# pass
#def JacoMo(xEst,u):
# return jMo
#def JacoOb(xEst):
# return