一、基本原理

卡尔曼滤波分为两大阶段：预测和更新。

1.1 预测

预测公式如下：
$x^{\prime}=Fx+B u$ $P^{\prime}=F P F^{T}+Q$
其中， $x^{\prime}$ 表示当前估计值； $F$ 表示状态转移矩阵； $x$ 表示上一时刻的最优估计值； $B$ 表示外部输入矩阵； $U$ 表示外部状态矩阵； $P^{\prime}$ 表示当前的先验估计协方差矩阵； $P$ 表示上一时刻的最优估计协方差矩阵； $Q$ 表示过程的协方差矩阵。

1.2 更新

1.2.1 写法一

更新公式如下：
$K=P^{\prime} H^{T}\left(H P^{\prime} H^{T}+R\right)^{-1}$ $x=x^{\prime}+K\left(z-H x^{\prime}\right)$ $P^{\prime}$
其中， $K$ 表示当前时刻的卡尔曼增益； $H$ 表示观测矩阵； $x$ 表示根据当前时刻的估计值及观测值融合得到的当前时刻的最优估计值； $z$ 表示实际测量值； $P$ 表示当前时刻的协方差矩阵； $I$ 表示单位矩阵。

1.2.2 写法二

更新公式如下：
$x^{\prime}$ $P^{\prime} H^{T}+R$ $K=P^{\prime} H^{T} S^{-1}$ $x=x^{\prime}+K y$ $P^{\prime}$
其中， $y$ 和 $S$ 无实际含义，属于中间变量； $K$ 表示当前时刻的卡尔曼增益； $H$ 表示观测矩阵； $x$ 表示根据当前时刻的估计值及观测值融合得到的当前时刻的最优估计值； $z$ 表示实际测量值； $P$ 表示当前时刻的协方差矩阵； $I$ 表示单位矩阵。

二、举例

本例子采用的是更新写法二
radar测量如下：
在这里插入图片描述

2.1 数据说明

在这里插入图片描述

2.2 代码

# 功能说明：
# 利用卡尔曼滤波来对radar数据滤波

import numpy as np
import matplotlib.pyplot as plt
from math import sin,cos,sqrt # sin,cos的输入是 弧度


if __name__ == "__main__":
    file = open('sample-laser-radar-measurement-data-2.txt') 
    # 数据变量初始化
    position_rho_measure = []
    position_theta_measure = []
    position_velocity_measure = []
    time_measure = []
    position_x_true = []  
    position_y_true = []
    speed_x_true = []
    speed_y_true = []
    
    # x,y方向的测量值(由非线性空间转到线性空间)
    position_x_measure = []
    position_y_measure = []
    speed_x_measure = []
    speed_y_measure = []

    # 先验估计值
    position_x_prior_est = []        # x方向位置的先验估计值
    position_y_prior_est = []        # y方向位置的先验估计值
    speed_x_prior_est = []           # x方向速度的先验估计值
    speed_y_prior_est = []           # y方向速度的先验估计值
    
    # 估计值和观测值融合后的最优估计值
    position_x_posterior_est = []    # 根据估计值及当前时刻的观测值融合到一体得到的最优估计值x位置值存入到列表中
    position_y_posterior_est = []    # 根据估计值及当前时刻的观测值融合到一体得到的最优估计值y位置值存入到列表中
    speed_x_posterior_est = []       # 根据估计值及当前时刻的观测值融合到一体得到的最优估计x速度值存入到列表中
    speed_y_posterior_est = []       # 根据估计值及当前时刻的观测值融合到一体得到的最优估计y速度值存入到列表中

    # 读取radar数据
    for line in file.readlines():    
        curLine = line.strip().split("	") 
        # 取出radar数据
        if curLine[0] == 'R': 
            position_rho_measure.append(float(curLine[1])) 
            position_theta_measure.append(float(curLine[2])) 
            position_velocity_measure.append(float(curLine[3])) 
            time_measure.append(float(curLine[4])) 
            position_x_true.append(float(curLine[5])) 
            position_x_true.append(float(curLine[6])) 
            speed_x_true.append(float(curLine[7])) 
            speed_y_true.append(float(curLine[8]))
            # 测量值 由非线性空间转到线性空间
            position_x_measure.append(float(curLine[1])*cos(float(curLine[2])))
            position_y_measure.append(float(curLine[1])*sin(float(curLine[2])))
            speed_x_measure.append(float(curLine[3])*cos(float(curLine[2])))
            speed_y_measure.append(float(curLine[3])*sin(float(curLine[2])))
    
    # --------------------------- 初始化 -------------------------
    # 用第二帧测量数据初始化
    X0 = np.array([[position_x_measure[1]],[position_y_measure[1]],[speed_x_measure[1]],[speed_y_measure[1]]])
    # 用第二帧初始时间戳
    last_timestamp_ = time_measure[1] 
    # 状态估计协方差矩阵P初始化（其实就是初始化最优解的协方差矩阵）
    P = np.array([[1.0, 0.0, 0.0, 0.0],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.0],
                  [0.0, 0.0, 0.0, 1.0]]) 

    X_posterior = np.array(X0)      # X_posterior表示上一时刻的最优估计值
    P_posterior = np.array(P)       # P_posterior是继续更新最优解的协方差矩阵
    
    # 将初始化后的数据依次送入(即从第三帧速度往里送)
    for i in range(2,len(time_measure)):
        # ------------------- 下面开始进行预测和更新，来回不断的迭代 -------------------------
        # 求前后两帧的时间差，数据包中的时间戳单位为微秒，处以1e6，转换为秒
        delta_t = (time_measure[i] - last_timestamp_) / 1000000.0
        last_timestamp_ = time_measure[i]
        # print("delta_t",delta_t)
        
        # 状态转移矩阵F，上一时刻的状态转移到当前时刻
        F = np.array([[1.0, 0.0, delta_t, 0.0],
                      [0.0, 1.0, 0.0, delta_t],
                      [0.0, 0.0, 1.0, 0.0],
                      [0.0, 0.0, 0.0, 1.0]])      

        # 控制输入矩阵B
        B = np.array([[delta_t*delta_t/2.0, 0.0],
                      [0.0, delta_t*delta_t/2.0],
                      [delta_t, 0.0],
                      [0.0, delta_t]])

        # 计算加速度也需要用估计的速度来做，而不是测量的速度
        # i = 4 开始，速度预测列表才会有2个值及以上
        if i == 2 or i == 3:
            acceleration_x_posterior_est = 0
            acceleration_y_posterior_est = 0
        else:
            acceleration_x_posterior_est = (speed_x_posterior_est[i-3] - speed_x_posterior_est[i-4])/delta_t
            acceleration_y_posterior_est = (speed_y_posterior_est[i-3] - speed_y_posterior_est[i-4])/delta_t
            
        # 控制状态矩阵U
        U = np.array([[acceleration_x_posterior_est],[acceleration_y_posterior_est]])

        # 打印测试
        print("acceleration_x_posterior_est",acceleration_x_posterior_est)
        print("acceleration_y_posterior_est",acceleration_y_posterior_est)

        # ---------------------- 预测  -------------------------
        # X_prior = np.dot(F,X_posterior) + np.dot(B,U)            # 使用加速度，X_prior表示根据上一时刻的最优估计值得到当前的估计值  X_posterior表示上一时刻的最优估计值
        X_prior = np.dot(F,X_posterior)                          # 不使用加速度，X_prior表示根据上一时刻的最优估计值得到当前的估计值  X_posterior表示上一时刻的最优估计值

        position_x_prior_est.append(X_prior[0,0])                # 将根据上一时刻计算得到的x方向最优估计位置值添加到列表position_x_prior_est中
        position_y_prior_est.append(X_prior[1,0])                # 将根据上一时刻计算得到的y方向最优估计位置值添加到列表position_y_prior_est中
        speed_x_prior_est.append(X_prior[2,0])                   # 将根据上一时刻计算得到的x方向最优估计速度值添加到列表speed_x_prior_est中
        speed_y_prior_est.append(X_prior[3,0])                   # 将根据上一时刻计算得到的x方向最优估计速度值添加到列表speed_y_prior_est中
        
        # Q:过程噪声的协方差，p(w)~N(0,Q)，噪声来自真实世界中的不确定性，N(0,Q) 表示期望是0，协方差矩阵是Q。Q中的值越小，说明预估的越准确。
        Q = np.array([[0.0001, 0.0, 0.0, 0.0],
                      [0.0, 0.00009, 0.0, 0.0],
                      [0.0, 0.0, 0.001, 0.0],
                      [0.0, 0.0, 0.0, 0.001]]) 
        
        # 计算状态估计协方差矩阵P
        P_prior_1 = np.dot(F,P_posterior)                        # P_posterior是上一时刻最优估计的协方差矩阵  # P_prior_1就为公式中的（F.Pk-1）
        P_prior = np.dot(P_prior_1, F.T) + Q                     # P_prior是得出当前的先验估计协方差矩阵      # Q是过程协方差

        # ------------------- 更新  ------------------------
        # 测量的协方差矩阵R，一般厂家给提供，R中的值越小，说明测量的越准确。
        R = np.array([[0.009, 0.0, 0.0, 0.0],
                      [0.0,  0.0009, 0.0, 0.0],
                      [0.0,  0.0,    9, 0.0],
                      [0.0,  0.0,    0, 9]])
    
        # 状态观测矩阵H（KF,radar,4*4）
        H = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

        # 计算卡尔曼增益K
        k1 = np.dot(P_prior, H.T)                                # P_prior是得出当前的先验估计协方差矩阵
        k2 = np.dot(np.dot(H, P_prior), H.T) + R                 # R是测量的协方差矩阵
        K = np.dot(k1, np.linalg.inv(k2))                        # np.linalg.inv()：矩阵求逆   # K就是当前时刻的卡尔曼增益

        # 测量值（数据输入的时候，就进行了线性化处理，从而可以使用线性H矩阵）
        Z_measure = np.array([[position_x_measure[i]],[position_y_measure[i]],[speed_x_measure[i]],[speed_y_measure[i]]])

        X_posterior_1 = Z_measure - np.dot(H, X_prior)           # X_prior表示根据上一时刻的最优估计值得到当前的估计值
        X_posterior = X_prior + np.dot(K, X_posterior_1)         # X_posterior是根据估计值及当前时刻的观测值融合到一体得到的最优估计值

        position_x_posterior_est.append(X_posterior[0, 0])       # 根据估计值及当前时刻的观测值融合到一体得到的最优估计x位置值存入到列表中
        position_y_posterior_est.append(X_posterior[1, 0])       # 根据估计值及当前时刻的观测值融合到一体得到的最优估计y位置值存入到列表中
        speed_x_posterior_est.append(X_posterior[2, 0])          # 根据估计值及当前时刻的观测值融合到一体得到的最优估计x速度值存入到列表中
        speed_y_posterior_est.append(X_posterior[3, 0])          # 根据估计值及当前时刻的观测值融合到一体得到的最优估计y速度值存入到列表中

        # 更新状态估计协方差矩阵P     （其实就是继续更新最优解的协方差矩阵）
        P_posterior_1 = np.eye(4) - np.dot(K, H)                 # np.eye(4)返回一个4维数组，对角线上为1，其他地方为0，其实就是一个单位矩阵
        P_posterior = np.dot(P_posterior_1, P_prior)             # P_posterior是继续更新最优解的协方差矩阵  # P_prior是得出的当前的先验估计协方差矩阵

    # 可视化显示
    if True:
        plt.rcParams['font.sans-serif'] = ['SimHei']  # 坐标图像中显示中文
        plt.rcParams['axes.unicode_minus'] = False
        

        # 一、绘制x-y图
        plt.xlabel("x")
        plt.ylabel("y")
        plt.plot(position_x_posterior_est, position_y_posterior_est, color='red', label="扩展卡尔曼滤波后的值")
        # plt.plot(position_x_true, position_y_true, color='green', label="真实值")
        plt.scatter(position_x_measure, position_y_measure, color='blue', label="测量值")
        plt.legend()  # Add a legend.
        plt.show()


        # 二、单独绘制x,y,Vx,Vy图像
        # fig, axs = plt.subplots(2, 2)

        # # axs[0,0].plot(position_x_true, "-", label="位置x_实际值", linewidth=1) 
        # axs[0,0].plot(position_x_measure, "-", label="位置x_测量值", linewidth=1) 
        # axs[0,0].plot(position_x_posterior_est, "-", label="位置x_扩展卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1)
        # axs[0,0].set_title("位置x")
        # axs[0,0].set_xlabel('k') 
        # axs[0,0].legend() 

        # # axs[0,1].plot(position_y_true, "-", label="位置y_实际值", linewidth=1) 
        # axs[0,1].plot(position_y_measure, "-", label="位置y_测量值", linewidth=1) 
        # axs[0,1].plot(position_y_posterior_est, "-", label="位置y_扩展卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1)
        # axs[0,1].set_title("位置y")
        # axs[0,1].set_xlabel('k') 
        # axs[0,1].legend() 
        
        # # axs[1,0].plot(speed_x_true, "-", label="速度x_实际值", linewidth=1) 
        # axs[1,0].plot(speed_x_measure, "-", label="速度x_测量值", linewidth=1) 
        # axs[1,0].plot(speed_x_posterior_est, "-", label="速度x_扩展卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1) 
        # axs[1,0].set_title("速度x")
        # axs[1,0].set_xlabel('k') 
        # axs[1,0].legend() 

        # # axs[1,1].plot(speed_y_true, "-", label="速度y_实际值", linewidth=1) 
        # axs[1,1].plot(speed_y_measure, "-", label="速度y_测量值", linewidth=1) 
        # axs[1,1].plot(speed_y_posterior_est, "-", label="速度y_扩展卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1)  
        # axs[1,1].set_title("速度y")
        # axs[1,1].set_xlabel('k') 
        # axs[1,1].legend() 

        # plt.show()