两个传感器卡尔曼滤波python优化实现

2021-08-02 10:59:35 阅读：315 来源： 互联网

标签：est 方差 python 卡尔曼滤波 predict 传感器 np z1

两个传感器卡尔曼滤波python优化实现

两个传感器滤波
优化：事先算好Kk
Python实现

两个传感器滤波

一辆车做变速直线运动，车上有GPS定位仪和速度表。通过卡尔曼滤波将两个传感器的数据对车的位置进行最优估计。假如观察到的数据为向量z=[z1 z2].T。 z1为GPS定位位置,z2为速度表的速度读数。z的协方差矩阵R~N(0,R)。车辆状态方程的噪声为wk~N(0,Q)。系统状态矩阵A=[1],预测值为上一个位置，最优估计为预测值，加测量值与预测值差乘以比例。卡尔曼滤波计算公式如下

在这里插入图片描述

优化：事先算好Kk

关键要计算k,就是测量差的分配比例，通过(1.11) (1.10) (1.13)三式可以计算k,而k仅和系统噪声方式Q
和测量误差协方差矩阵R有关。因此完全可以事先迭代几次先计算出来k,然而对大量的数据计算位置估计。只需要计算(1.9)式和(1.12)式。k事先已算好。由卡尔曼五步简化为2步，大大减少矩阵计算量。

Python实现

假如车辆直线位置真实的方程为100 *(1 - np.exp(-t/10)), 传感器z1的方差为2,速度表的z2的方差为0.2,
R=[[2 0] ,系统噪声方差Q=[1]。系统状态A=[1],观察矩阵H=[1 1].T
[0 0.2]]

函数pre_kalman，通过参数A H Q R ,可以事先算好卡尔曼滤波参数K_K。迭代几次就收敛了。
函数obv()按方程100 *(1 - np.exp(-t/10))生成汽车真实位置，再分别入加入高斯分布噪声,返回传感器观察
的数据z。z0为GPS位置数据，z1是速度。需要对z1进行积分得到位置。然后依据z和k_k进行卡尔曼滤波估计。

一般GPS受干扰位置方差大，速度表方差小但是积分后累积误差大。从图上看卡尔曼滤波估计值拟合真实值很好。如图
在这里插入图片描述
附程序

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from matplotlib.font_manager import FontProperties

# 预设字体
font = {'family': 'SimHei', "size": 14}
matplotlib.rc('font', **font)

'''
卡尔曼滤波，2路观察
'''

'''
真实
x_k = A*xk_1 + w_k  w_k方差Q
观察
z_k = H*x_k + v_k   v_k方差R

预测
x_k_predict = A*x_k_est
p_k_predict = A * p_k_est * A_T + Q
k_k = p_k_predict * H_T *( H*p_k_predict*H_T + R)^-1

最优估计
x_k_est = x_k_predict + k_k *( z_k - H*x_k_predict)
p_k_est = (I - k_k * H) * p_k_predict 
'''

# 系统状态A
A = np.mat([1])

# 两个观察仪器的观察矩阵
H = np.mat([[1], [1]])

#观察仪器方差sigma1 sigma2
sigma1 = 2
sigma2 = 0.2

#系统噪声wk方差Q
Q = np.mat(1)

'''由Q R算kk'''
def pre_kalman(A,H):
    R=np.mat([[sigma1, 0],[0, sigma2]])
    I=np.mat([1])
    p_est = np.mat([0]) #估计协方差矩阵
    for _ in np.arange(0,10,1):
        p_predict = A * p_est * A.T + Q #预测协方差矩阵
        kk = p_predict * H.T * (H * p_predict * H.T + R).I
        p_est = (I - kk * H) * p_predict
    return kk #返回k_k

'''
两个测量仪器测量两组信号,z0为位置测量值，z1为速度测量值
z0的方差为sigma1,z1方差为sigma2
真实信号为1-exp(t/10)
'''
def obv():
    t = np.linspace(1,100,100)
    real = 100 *(1 -  np.exp(-t/10))
    #real = t**2

    #求速度xdot
    dt = t[1:] - t[:-1]
    dx = real[1:] - real[:-1]
    xdot = dx / dt
    #补第0个对齐
    xdot = np.hstack([real[0],xdot])

    #真实信号实噪声，模拟测量仪器的误差噪声
    t = t[:-1]
    real=real[:-1]
    xdot=xdot[:-1]
    z1= real + np.random.normal(0,sigma1,size=t.shape[0])
    z2= xdot + np.random.normal(0,sigma2,size=t.shape[0])
    z = np.mat([(a,b) for (a,b) in zip(z1,z2)])
    return [t, dt, real,z]


'''
计算卡尔曼
z是观察到的数据
kk是卡尔慢比例系数
'''
def calc_kalman(A,H,z_k,kk):
    global  x_est
    x_pred = A * x_est
    x_est = x_pred + kk * (z_k - H * x_pred)
    return x_est


global x_est #最佳估计值
if __name__ == '__main__':

    #算k值
    kk = pre_kalman(A,H)

    #生成观察仪器信息Z，返回时间,真实值，两个观察仪器带噪声的值
    [t, dt, xreal, z] = obv()

    x_est=np.mat([0])
    x_est_list=[]

    z1_integral=0
    for k in range(z.shape[0]):
        #对速度进行积分
        z1_integral += z[k][0,1] * dt[k]
        z[k][0,1] = z1_integral

        #计算卡尔曼,结果在x_est
        calc_kalman(A, H, z[k].T, kk)
        x_est_list.append(x_est[0,0])

    x_est_list=np.array(x_est_list)


    plt.title('卡尔曼滤波')
    plt.plot(t, xreal, label='真实',color='g')
    plt.plot(t, z[:,0], label='位置测量',color='b')
    plt.plot(t, z[:,1], label='速度积分',color='y')
    plt.plot(t, x_est_list, label='估计',color='r')
    plt.legend(loc="lower right")
    plt.show()

标签：est,方差,python,卡尔曼滤波,predict,传感器,np,z1
来源： https://blog.csdn.net/qq_23117711/article/details/119320245

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

两个传感器卡尔曼滤波python优化实现