代码
learn_kalman.py
#coding=utf-8
import numpy as np
import time
from kinematic_model import freedrop
from controller import kalman_filter
import matplotlib.pyplot as plt
# 支持中文
import matplotlib as mpl
mpl.rcParams['font.family']='SimHei'
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
class Scene:
'''
场景
'''
def __init__(self,windSpd=np.array([0.7,0.3,0.0]),\
initialSpd=np.array([120.0,0.0,120.0])):
'''
我现在是一个防空兵
防空炮打出一枚炮弹，真实的炮弹轨迹，它可能会受风的影响，可能会有随机因素导致偏离目标导致打不中飞机...
我们可以使用指挥所观测到的炮弹轨迹，因为炮弹距离很远，所以这个观测不是很靠谱...
所以我们所使用了卡尔曼滤波算法，得到了一条真实的炮弹轨迹...
'''
# 真实的炮弹
self.realShell=freedrop.FreeDropBinder(windSpd=windSpd,initialSpd=initialSpd)
# 理论上炮弹的落点
self.theoShell=freedrop.FreeDropBinder(windSpd=np.array([0.0,0.0,0.0]),initialSpd=initialSpd,randRatio=0.0)
# 卡卡尔曼滤波器
self.kf=kalman_filter.KF_Onmi3D()
self.kf.initState[3:6]=initialSpd
# 绘图区
self.fig=plt.figure('炮弹弹道图')
self.ax = self.fig.gca(projection="3d")
# 数据缓存
self.realCoord=[]
self.theoCoord=[]
self.kalmanCoord=[]
self.observeCoord=[]
def UpdateData(self,delta_t=0.2):
'''
更新虚拟环境的数据
:return:
'''
# 真实炮弹轨迹
self.realShell.StateUpdate(delta_t=delta_t)
# 理论炮弹轨迹
self.theoShell.StateUpdate(delta_t=delta_t)
# 观测到的炮弹轨迹
self.observeCoord.append(self.realShell.position + np.random.random(3) * self.realShell.position[0]/20.0 - self.realShell.position[0]/40.0)
# 卡尔曼滤波
'''
基于卡尔曼滤波，结合理论炮弹轨迹 对观测的炮弹轨迹进行修正
'''
self.kf.Predict(velocity=self.theoShell.spd)
Hybrid_Position=self.kf.Update(self.observeCoord[-1])
# 绘图(真实的弹道)
plt.cla()
self.ax.set_xlim(0, 1000)
self.ax.set_ylim(-200, 200)
self.ax.set_zlim(0, 300)
self.ax.set_xlabel("X坐标（米）")
self.ax.set_ylabel("Y坐标（米）")
self.ax.set_zlabel("X坐标（米）")
# 计算三个类型的炮弹
self.realCoord.append(np.copy(self.realShell.position)) # 真实炮弹
self.theoCoord.append(np.copy(self.theoShell.position)) # 理论模型
self.kalmanCoord.append(np.copy(Hybrid_Position))
self.curve2Draw=np.array(self.realCoord)
self.curve2 = np.array(self.observeCoord)
self.curve3 = np.array(self.theoCoord)
self.curve4 = np.array(self.kalmanCoord)
self.ax.plot(self.curve2Draw[:,0],self.curve2Draw[:,1],self.curve2Draw[:,2],label='真实炮弹',color='red')
self.ax.scatter(self.curve2[:, 0], self.curve2[:, 1], self.curve2[:, 2],'rv+', label='炮弹观测数据', color='blue',alpha=0.5,s=1)
self.ax.plot(self.curve3[:, 0], self.curve3[:, 1], self.curve3[:, 2], label='炮弹理论轨迹', color='green', alpha=0.5)
self.ax.plot(self.curve4[:, 0], self.curve4[:, 1], self.curve4[:, 2], label='炮弹融合轨迹', color='yellow', alpha=1.0)
self.ax.legend()
plt.pause(0.05)
# 开始模拟环境
#plt.ion()
s=Scene()
for i in range(1000):
if s.realShell.position[2]<0: break
s.UpdateData()
plt.ioff()
plt.show()
freerop.py
#coding=utf-8
import time
import numpy as np
'''
3D自由落体模型（含有风阻）
'''
class FreeDropBinder:
'''
为实体绑定自由落体属性
'''
def __init__(self,windSpd=np.array([0.0,0.0,0.0]),
resRatio=0.0004,
G=9.8,
initialPos=np.array([0.0,0.0,0.0]),
initialSpd=np.array([0.0,0.0,0.0]),
randRatio=0.1):
'''
:param windSpd: 风速（三维）
:param resRatio: 风阻比例（全向）
:param G: 重力加速度
:param initialPos: 物体初始位置
:param initialSpd: 物体初始速度
'''
self.position=initialPos
self.spd=initialSpd
self.windSpd=windSpd
self.resRatio=resRatio
self.G=G
self.randRatio=randRatio
def StateUpdate(self,delta_t=0.05,driveForce=np.array([0.0,0.0,0.0])):
'''
更新实体位置信息
:param delta_t:
:return:
'''
# 重力因素
self.spd+=np.array([0,0,-self.G*delta_t])
# 风阻因素
self.spd=np.where(self.spd>0,self.spd-self.resRatio*self.spd*self.spd,self.spd)
self.spd = np.where(self.spd <= 0, self.spd + self.resRatio * self.spd * self.spd, self.spd)
# 风力因素
# 驱动因素
self.spd+=(driveForce+self.windSpd)*delta_t
# 随机因素
self.spd+=(np.random.rand(3)-0.5)*2*self.randRatio*delta_t
# 更新坐标
self.position=self.position+self.spd*delta_t
if __name__=='__main__':
box=FreeDropBinder(initialSpd=np.array([10.0,0.0,100.0]))
for i in range(30):
print(box.Update())
kalman_filter.py
import numpy as np
import matplotlib.pyplot as plt
class KF_Onmi3D:
'''
三维，无方向场景下的卡尔曼滤波算法模组
'''
def __init__(self):
# 初始状态 x y z vx vy vz
self.initState=np.array([0, 0, 0, 0, 0, 0],dtype=np.float)
# 初始协方差,可以看出是每个维度都是一一对应的关系
'''
[ 1 0 0 0 0 0 ]
[ 0 1 0 0 0 0 ]
[ 0 0 1 0 0 0 ]
[ 0 0 0 1 0 0 ]
[ 0 0 0 0 1 0 ]
[ 0 0 0 0 0 1 ]
'''
self.initCov=np.eye(6)
# 状态转移矩阵
self.stateTransMatrix=np.array([[1,0,0,1,0,0],
[0,1,0,0,1,0],
[0,0,1,0,0,1],
[0,0,0,1,0,0],
[0,0,0,0,1,0],
[0,0,0,0,0,1]],dtype=np.float)
# 观测矩阵 X Y Z Vx Vy Vz
self.observeMatrix=np.array([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0]],dtype=np.float)
# 过程噪声（先设定一个初始值，这个需要跟据你系统的评估来确定）
self.procNoise=np.eye(6)*0.001
# 观测噪声的协方差矩阵
self.observeNoiseCov=np.eye(3)*1
self.InitParams()
def InitParams(self):
'''
初始化状态变量
:return:
'''
self.currentState=self.initState.copy()
self.predictState=self.initState.copy()
self.currentCov=self.initCov
self.predictedCov=self.currentCov
def Predict(self,velocity=np.array([0,0,0],dtype=np.float)):
'''
预测过程
:param v:
:return:
'''
# 基于当前的速度，预测机器人下一个状态的状态数值
self.predictState=self.stateTransMatrix.dot(self.currentState)
# 预测三维环境下的协方差矩阵
self.predictedCov=self.stateTransMatrix.dot(self.currentCov).dot(self.stateTransMatrix.T)+self.procNoise
# 把速度赋值给状态中的“速度”属性
self.currentState[3:6] = velocity
def Update(self,observed_Pos=np.array([0,0,0],dtype=np.float)):
'''
更新数据
:param observed_Pos: 带有误差的位置观测值
:return:
'''
# 卡尔曼增益（Kalman Gain）计算
'''
K=\frac{估计的误差}{估计的误差+测量的误差}=\frac{\hat{P_k}C}{C\hat{P_k}C^T+Error}
'''
self.Kalman_Gain = self.predictedCov.dot(self.observeMatrix.T) \
.dot(np.linalg.inv( \
self.observeMatrix.dot(self.predictedCov).dot(self.observeMatrix.T) + self.observeNoiseCov))
'''
基于Kalman Gain估算当前状态
'''
self.currentState = self.predictState + self.Kalman_Gain.dot(observed_Pos-self.observeMatrix.dot(self.predictState))
'''
当前协方差估计
'''
self.currentCov = (np.eye(6) - self.Kalman_Gain.dot(self.observeMatrix)).dot(self.predictedCov)
return self.currentState[0:3]