扩展卡尔曼滤波算法——基本原理及举例（python实现radar数据滤波）

一、基本原理

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

1.1 预测

预测公式如下：
$$x^{\prime }=Fx+Bu$$ $$P^{\prime }=FP{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=(I-KH)P^{\prime }$$
其中，$K$ 表示当前时刻的卡尔曼增益；$H$ 表示观测矩阵；$x$ 表示根据当前时刻的估计值及观测值融合得到的当前时刻的最优估计值；$z$ 表示实际测量值；$P$ 表示当前时刻的协方差矩阵；$I$ 表示单位矩阵。

1.2.2 写法二

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

1.2.3 扩展卡尔曼滤波和卡尔曼滤波的差异

扩展卡尔曼滤波和卡尔曼滤波的差异在于观测矩阵 $H$ 存在差异，扩展卡尔曼滤波需要将原来的非线性数据转化到线性数据，采用雅克比矩阵，H矩阵的功能是线性化。

二、举例

本例子采用的是更新写法二。
radar测量如下：

2.1 数据说明

2.2 观测矩阵H

1、估计量格式：

2、测量值格式：

3、为了使之能做差值运算，需要用H矩阵将其线性化，统一到同一维度，用估计量带入求出观测矩阵H。

2.3 代码

# 功能说明：
# 利用扩展卡尔曼滤波来对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.000009, 0.0,    0.0],
                      [0.0,  0.009, 0.0],
                      [0.0,  0.0,    9]])

        # 状态观测矩阵H (EKF，radar, 3*4）
        Px = X_prior[0,0]
        Py = X_prior[1,0]
        Vx = X_prior[2,0]
        Vy = X_prior[3,0]

        # 避免被除数为0
        position_rho_ = sqrt(X_prior[0,0]*X_prior[0,0]+X_prior[1,0]*X_prior[1,0])
        if position_rho_ < 1e-8:
            position_rho_ = 1e-8

        # 线性化(将非线性转为线性)
        # 计算H矩阵，需要用估计的位置、速度来做，而不是测量的位置、速度
        H = np.array([[Px/position_rho_, Py/position_rho_, 0.0, 0.0],
                      [-Py/(position_rho_*position_rho_), Px/(position_rho_*position_rho_), 0.0, 0.0],
                      [Py*(Vx*Py-Vy*Px)/(position_rho_*position_rho_*position_rho_), \
                      Px*(Vy*Px-Vx*Py)/(position_rho_*position_rho_*position_rho_), Px/position_rho_, Py/position_rho_]])

        # 计算卡尔曼增益
        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就是当前时刻的卡尔曼增益

        # 测量值
        Z_measure = np.array([[position_rho_measure[i]],[position_theta_measure[i]],[position_velocity_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()

2.4 实验结果

注意：

1. 图中测量值和预测值横坐标尚未对齐。测量值的前两个值因未参与预测，故需要舍去，这样绘图才能保证对齐。

三、调参技巧

1. 一般来说，上一时刻数据与这一时刻数据的间隔时间一般是已知的，若不已知，则需要想办法得到间隔时间。（不同的间隔时间，对后续的其他参数有影响）
2. 调参主要调协方差矩阵P的初始值、过程的协方差矩阵Q、测量的协方差矩阵R。
3. 先调好测量的协方差矩阵R，R一般是厂家给出的，其数值越小，代表测量精度越高。
4. 再调调协方差矩阵P的初始值。
5. 最后调过程的协方差矩阵Q。

四、小贴士

1. 计算外部状态$U$的时候，不能用预测值来计算，得用估计值。
2. 计算观测矩阵$H$的时候，不能用预测值来计算，得用估计值。

参考资料

1. 卡尔曼滤波简介
2. 快速上手的Python版二维卡尔曼滤波解析
3. 多传感器融合定位1（激光雷达+毫米波雷达）
4. 使用卡尔曼滤波和扩展卡尔曼滤波进行毫米波雷达和激光雷达数据融合示例