代码

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]

参考

https://www.bilibili.com/video/BV1gF411f78t/?spm_id_from=333.337.top_right_bar_window_history.content.click&vd_source=667c3d14dbb51ec849c0bc7c38329d10