python卡尔曼多维_Python 实现 卡尔曼滤波器 --非常简单

整体思路很简单，卡尔曼滤波器就是做数据融合的，先给一个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 jOb

def JacoMo(x, u):

"""

Jacobian of Motion Model

motion model

x_{t+1} = x_t+v*dt*cos(yaw)

y_{t+1} = y_t+v*dt*sin(yaw)

yaw_{t+1} = yaw_t+omega*dt

v_{t+1} = v{t}

so

dx/dyaw = -v*dt*sin(yaw)

dx/dv = dt*cos(yaw)

dy/dyaw = v*dt*cos(yaw)

dy/dv = dt*sin(yaw)

"""

yaw = x[2, 0]

v = u[0, 0]

jF = np.matrix([[1.0, 0.0, -dt * v * math.sin(yaw), dt * math.cos(yaw)],

[0.0, 1.0, dt * v * math.cos(yaw), dt * math.sin(yaw)],

[0.0, 0.0, 1.0, 0.0],

[0.0, 0.0, 0.0, 1.0]])

return jF

def JacoOb(x):

# Jacobian of Observation Model

jH = np.matrix([

[1, 0, 0, 0],

[0, 1, 0, 0]

])

return jH

def ekf_estimation(xEst,pEst,z,u):

# yu ce predict

xPre=motion_model(xEst,u)

jMo=JacoMo(xEst,u) # Jacobin of motion model

jOb=JacoOb(xEst)

pPre=jMo*pEst*jMo.T+Q

s=jOb*pPre*jOb.T+R

k=pPre*jOb.T*np.linalg.inv(s)

# update estimate

H=np.matrix([

[1,0,0,0],

[0,1,0,0]

])

zPre=H*xPre

xEst=xPre+k*(z-zPre)

pEst=(np.eye(len(xEst)) - k*jOb)*pPre

return xEst,pEst

def input_data(xTrue,xImu):

u=np.matrix([1.0,0.1]).T#jiaosudu he xiansudu

xTrue=motion_model(xTrue,u)

Qsim=np.diag([0.5,0.5])**2

Rsim=np.diag([1.0,math.radians(5.0)])**2

#GPS

z=np.matrix([xTrue[0,0]+np.random.randn()*Qsim[0,0],

xTrue[1,0]+np.random.randn()*Qsim[1,1]]).T

ud=np.matrix([u[0,0]+np.random.randn()*Rsim[0,0],

u[1,0]+np.random.randn()*Rsim[1,1]]).T

xImu=motion_model(xImu,ud)

return ud,z,xTrue,xImu

if __name__=="__main__":

xEst=np.matrix(np.zeros((4,1)))

xTrue=xEst

xImu=xTrue

pEst=np.eye(4)

t=0.1

hTrue=xTrue

hEst=xEst

hz=np.zeros((2,1))

hImu=xImu

for i in range(1000):

u,z,xTrue,xImu=input_data(xTrue,xImu)

xEst,pEst=ekf_estimation(xEst,pEst,z,u)

hTrue=np.hstack((hTrue,xTrue))

hEst=np.hstack((hEst,xEst))

hImu=np.hstack((hImu,xImu))

hz=np.hstack((hz,z))

# plot

plt.cla()

plt.plot(hz[0,:],hz[1,:],".g")

plt.plot(np.array(hTrue[0,:]).flatten(),

np.array(hTrue[1,:]).flatten(),"-b")

plt.plot(np.array(hEst[0,:]).flatten(),

np.array(hEst[1,:]).flatten(),"-r")

plt.plot(np.array(hImu[0,:]).flatten(),

np.array(hImu[1,:]).flatten(),"-k")

plt.pause(0.001)

运行结果：