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

2022-03-10 06:27:20

两个传感器卡尔曼滤波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()

码农公寓