变量名	代码中对应值
状态变量的真实值 $\small X_{k}$	X_true
状态变量的分量
X坐标的真实值	position_x_true
Y坐标的真实值	position_y_true
X方向速度的真实值	speed_x_true
Y方向速度的真实值	speed_y_true
X方向加速度的真实值	acceleration_x_true
Y方向加速度的真实值	acceleration_y_true
测量值的真实值
测量值 $\small Z_{k}$	Z
测量值的分量（均为真实值）	注意我们观察的时候，只测xy坐标两个变量
X坐标的真测量值	position_x_measure
Y坐标的真测量值	position_y_measure
X方向速度的真测量值	speed_x_measure
Y方向速度的真测量值	speed_y_measure
X方向加速度的真测量值	acceleration_x_measure
Y方向加速度的真测量值	acceleration_y_measure
噪声
过程噪声 $\small W_{k}\sim N(0,Q)$	W
X坐标的噪声	w_position_x
Y坐标的噪声	w_position_y
X方向速度的噪声	w_speed_x
Y方向速度的噪声	w_speed_y
X方向加速度的噪声	w_acceleration_x
Y方向加速度的噪声	w_acceleration_y
测量噪声 $\small V_{k}\sim N(0,R)$	V
X坐标的测量噪声	v_position_x
Y坐标的测量噪声	v_position_y
系数矩阵
状态矩阵A	A
传输矩阵H	H

2.1.2 基于卡尔曼滤波的五步迭代要建立以下变量

变量名	代码中对应变量
状态变量
状态变量的先验估计值 $\small \hat{X_{k}^{-}}$	X_prior
状态变量的后验估计值 $\small \hat{X_{k}}$	X_ posteriori

协方差矩阵
误差ek的协方差矩阵Pk	P
误差ek的协方差矩阵Pk的先验	P_prior
过程噪声w的协方差矩阵Q	Q
测量噪声v的协方差矩阵R	R

卡尔曼增益 $\small K_{k}$	K

2.1.3 分析各个变量的初始值

本例的代码参考了：卡尔曼滤波理论讲解与应用（matlab和python）_O_MMMM_O的博客-CSDN博客_ekf卡尔曼滤波

为了方便后续计算，设 $\small \Delta t=dt$

不妨设初始位置 $\small X_{1,0}=0,X_{2,0}=0,$ 初始速度v=5,加速度a=4,偏航角设置为 $\theta =\pi /3$

通过设置偏航角 ${\color{Red} \theta}$ 来将矢量的坐标/速度分到X轴和Y轴上。

（1）所以k-1时刻的X_true的初始值设置为：

X_true = np.array([[0],[0],[v_0 * math.cos(theta)],[v_0 * math.sin(theta)],[a_0 * math.cos(theta)],[a_0 * math.sin(theta)]])

（2）k-1时刻的状态变量的后验估计可以直接取真实值

X_posteriori = X_true

（3）过程噪声的协方差矩阵Q设置为

（4）测量噪声的协方差矩阵R设置为：

要注意我们只测量X的前两个变量，即X坐标和Y坐标。（定位更关心位置而不是速度）

即：更相信建模值，而较为不信任测量值。

（5）状态转移矩阵A:(t就是时间间隔，可以对其取值)

#状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1,0,t,0,0.5*t*t,0],[0,1,0,t,0,0.5*t*t],[0,0,1,0,t,0],[0,0,0,1,0,t],[0,0,0,0,1,0],[0,0,0,0,0,1]])
#传输矩阵/状态观测矩阵H
H = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0]])

（6）注意传输矩阵H，因为我们只测量了X坐标和Y坐标，所以它的维度是2*6。

（7）滤波误差的协方差矩阵的初始值对算法影响不大，因为算法迭代的过程，就是协方差矩阵不断变小的过程。初始值我们取：

var_p = 3
P = np.eye(6) * var_p

2.1.4算法迭代

（1）首先是要表达出真实值，注意，真实值=理论值+噪声

X状态空间：

 X_true = np.dot(A, X_true) + W

测量值：

 Z = np.dot(H, X_true) + V

（2）然后就是五步迭代，可以参考上一篇，不过多赘述。

2.2 Python源代码

import math
import numpy as np
from matplotlib import pyplot as plt

plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号

#-----------------创建一个函数，用来生成符合正态分布的变量------------------------
def Gaussian_variable_generator(sigma):
    y=np.random.normal(loc=0.0,scale=sigma,size=None)
    return np.array(y)

#-----------------建立变量-------------------------------------------------
# 设时间间隔为t

# 过程噪声的协方差矩阵
t=0.08
# G = np.array([1/6*t*t*t,0],[0,1/6*t*t*t],[1/2*t*t,0],[0,1/2*t*t],[t,0],[0,t])
# G1 = np.array([],[])
# 过程噪声的方差
var_q = 0.04
Q = np.eye(6) * var_q
# print(Q)
sigma_w1 = math.sqrt(Q[0,0])
sigma_w1 = np.array(sigma_w1)
sigma_w2 = math.sqrt(Q[1,1])
sigma_w2 = np.array(sigma_w2)
sigma_w3 = math.sqrt(Q[2,2])
sigma_w3 = np.array(sigma_w3)
sigma_w4 = math.sqrt(Q[3,3])
sigma_w4 = np.array(sigma_w4)
sigma_w5 = math.sqrt(Q[4,4])
sigma_w5 = np.array(sigma_w5)
sigma_w6 = math.sqrt(Q[5,5])
sigma_w6 = np.array(sigma_w6)

# 测量噪声的协方差矩阵
# 因为只有两个测量量：X坐标和Y坐标
R = np.array([[4,0],[0,4]])
sigma_v1 = math.sqrt(R[0,0])
sigma_v1 = np.array(sigma_v1)
sigma_v2 = math.sqrt(R[1,1])
sigma_v2 = np.array(sigma_v2)

#状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1,0,t,0,0.5*t*t,0],[0,1,0,t,0,0.5*t*t],[0,0,1,0,t,0],[0,0,0,1,0,t],[0,0,0,0,1,0],[0,0,0,0,0,1]])
#传输矩阵/状态观测矩阵H
H = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0]])

#----------------------初始化------------------------------------------

#设初始的真实值 X坐标=0 Y坐标=0 初速度v=10 加速度a=2 偏航角theta=pi/3
v_0 = 5
a_0 = 4
theta = math.pi / 3
#利用偏航角 把X_true的各个分量表达出来
X_true = np.array([[0],[0],[v_0 * math.cos(theta)],[v_0 * math.sin(theta)],[a_0 * math.cos(theta)],[a_0 * math.sin(theta)]])
print(X_true)

# position_x_true = v_0 * math.cos(theta) * t + 0.5 * a_0 * math.cos(theta) * t * t
# position_y_true = v_0 * math.sin(theta) * t + 0.5 * a_0 * math.sin(theta) * t * t
# speed_x_true = v_0 * math.cos(theta) + a_0 * math.cos(theta) * t
# speed_y_true = v_0 * math.sin(theta) + a_0 * math.sin(theta) * t
# acceleration_x_true = a_0 * math.cos(theta)
# acceleration_y_true = a_0 * math.sin(theta)


#k-1时刻的状态变量的后验估计可以直接取真实值
X_posteriori = X_true

#假设 k-1 时刻的误差的协方差矩阵为:(误差的协方差矩阵的初始值不是特别重要 反正都会收敛)
var_p = 3
P = np.eye(6) * var_p

#X Y坐标的先验估计值
position_x_prior = []
position_y_prior = []

#X Y方向的速度的先验估计值
speed_x_prior = []
speed_y_prior = []

#X Y方向的加速度的先验估计值
acceleration_x_prior = []
acceleration_y_prior = []

#测量的量只有两个：X坐标和Y坐标
#X坐标的测量量
position_x_measure = []
#Y坐标的测量量
position_y_measure = []

#X Y坐标的后验估计值
position_x_posteriori = []
position_y_posteriori = []

#X Y方向的速度的后验估计值
speed_x_posteriori = []
speed_y_posteriori = []

#X Y方向的加速度的后验估计值
acceleration_x_posteriori = []
acceleration_y_posteriori = []

#X Y坐标的真实值
position_x_true = X_true[0,0]
position_y_true = X_true[1,0]

#X Y方向的速度的后验估计值
speed_x_true = X_true[2,0]
speed_y_true = X_true[3,0]

#X Y方向的加速度的后验估计值
acceleration_x_true = X_true[4,0]
acceleration_y_true = X_true[5,0]

#误差的协方差的迹 用来看是否已经达到最小方差的估计准则了
tr_P_posterior = P[0, 0] + P[1, 1] + P[2, 2] + P[3, 3]+P[4, 4] + P[5, 5]

if __name__ == '__main__':
    #迭代次数设为30次
    for i in range(100):
    # 基于状态空间表达式要建立以下方程(这里全是真实值啊！！)
    # 这里要注意 要在循环里面生成每一次的噪声 否则噪声永远都是一个值
        w1 = Gaussian_variable_generator(sigma_w1)
        w2 = Gaussian_variable_generator(sigma_w2)
        w3 = Gaussian_variable_generator(sigma_w3)
        w4 = Gaussian_variable_generator(sigma_w4)
        w5 = Gaussian_variable_generator(sigma_w5)
        w6 = Gaussian_variable_generator(sigma_w6)
        W = np.array([[w1], [w2], [w3], [w4], [w5], [w6]])
        print(W)
    #真实值的表达式
        X_true = np.dot(A, X_true) + W
        position_x_true = np.append(position_x_true, X_true[0,0])
        position_y_true = np.append(position_y_true, X_true[1,0])
        speed_x_true = np.append(speed_x_true, X_true[2,0])
        speed_y_true = np.append(speed_y_true, X_true[3,0])
        acceleration_x_true = np.append(acceleration_x_true, X_true[4,0])
        acceleration_y_true = np.append(acceleration_y_true, X_true[5,0])

    # 测量噪声
        v1 = Gaussian_variable_generator(sigma_v1)
        v2 = Gaussian_variable_generator(sigma_v2)
        V = np.array([[v1], [v2]])

    # 测量值的表达式
        Z = np.dot(H, X_true) + V
        position_x_measure = np.append(position_x_measure, Z[0,0])
        position_y_measure = np.append(position_y_measure, Z[1,0])

# ----------------------开始循环迭代-------------------------
# ----------------------时间更新-------------------------

    # step1:基于k-1时刻的后验估计值X_posterior建模预测k时刻的系统状态先验估计值X_prior
    # 对状态变量进行先验估计
        X_prior = np.dot(A, X_posteriori)
    # 为了看到Z状态空间各个分量的先验值的变化
        position_x_prior.append(X_prior[0,0])
        position_y_prior.append(X_prior[1,0])
        speed_x_prior.append(X_prior[2,0])
        speed_y_prior.append(X_prior[3,0])
        acceleration_x_prior.append(X_prior[4,0])
        acceleration_y_prior.append(X_prior[5,0])

    # step2:基于k-1时刻的误差ek-1的协方差矩阵P_posterior和过程噪声w的协方差矩阵Q 预测k时刻的误差的协方差矩阵的先验估计值 P_prior
        P_prior = np.dot(A,np.dot(P, A.T)) + Q

# ----------------------状态更新-------------------------
    # step3:计算卡尔曼增益
        K1 = np.dot(P_prior, H.T)
        K2 = np.dot(H, np.dot(P_prior,H.T)) + R
        K = np.dot(K1, np.linalg.inv(K2))

    # step4:利用卡尔曼增益K 进行校正更新状态，得到k时刻的后验状态估计值 X_posterior
        X_posteriori = X_prior + np.dot(K, Z - np.dot(H, X_prior))
    # 把k时刻后验预测值赋给状态分量的后验预测值
    # X Y坐标的后验估计值
        position_x_posteriori.append(X_posteriori[0,0])
        position_y_posteriori.append(X_posteriori[1,0])
    # X Y方向的速度的后验估计值
        speed_x_posteriori.append(X_posteriori[2,0])
        speed_y_posteriori.append(X_posteriori[3,0])
    # X Y方向的加速度的后验估计值
        acceleration_x_posteriori.append(X_posteriori[4,0])
        acceleration_y_posteriori.append(X_posteriori[5,0])

    # step5:更新误差的协方差矩阵
        P = np.dot(np.eye(6)-np.dot(K, H), P_prior)
    # 误差的协方差矩阵的迹，迹越小说明估计越准确
        tr_P_posterior = np.append(tr_P_posterior, P[0, 0] + P[1, 1] + P[2, 2] + P[3, 3] + P[4, 4] + P[5, 5])
   # ---------------------再从step5回到step1 其实全程只要搞清先验后验 k的自增是隐藏在循环的过程中的 然后用分量speed和position的append去记录每一次循环的结果-----------------------------

    # 可视化显示 画出误差的迹 观察其是否收敛
    if True:
        # 画出1行2列的多子图

        fig, axs = plt.subplots(1,4,figsize=(20,14))

        #速度
        axs[2].plot(speed_y_true,"-",color="blue",label="Y速度真实值",linewidth="1")
        # axs[2].plot(speed_measure,"-",color="grey",label="速度测量值",linewidth="1")
        axs[2].plot(speed_y_prior,"-",color="green",label="Y速度先验估计值",linewidth="1")
        axs[2].plot(speed_y_posteriori,"-",color="pink",label="Y速度后验估计值",linewidth="1")
        axs[2].set_title("Y轴方向速度")
        axs[2].set_xlabel('k')
        axs[2].legend(loc = 'upper left')

        axs[3].plot(speed_x_true, "-", color="blue", label="X速度真实值", linewidth="1")
        # axs[2].plot(speed_measure,"-",color="grey",label="速度测量值",linewidth="1")
        axs[3].plot(speed_x_prior, "-", color="green", label="X速度先验估计值", linewidth="1")
        axs[3].plot(speed_x_posteriori, "-", color="pink", label="X速度后验估计值", linewidth="1")
        axs[3].set_title("X轴方向速度")
        axs[3].set_xlabel('k')
        axs[3].legend(loc='upper left')

        font2 = {
                 'weight': 'bold',

                 'size': 20,

                 }


        # # 位置
        x = position_x_true
        y = position_y_true
        axs[1].plot(x, y,"-",color="blue",label="位置真实值",linewidth="1")

        x1 = position_x_measure
        y1 = position_y_measure
        axs[1].plot(x1,y1,"-",color="grey",label="位置测量值",linewidth="1")

        x2 = position_x_prior
        y2 = position_y_prior
        axs[1].plot(x2,y2,"-",color="green",label="位置先验估计值",linewidth="1")

        x3 = position_x_posteriori
        y3 = position_y_posteriori
        axs[1].plot(x3,y3,"-",color="red",label="位置后验估计值",linewidth="1")
        axs[1].set_title("轨迹图",font2)
        axs[1].set_ylabel("Y坐标/m",font2)
        axs[1].set_xlabel('X坐标/m',font2)
        axs[1].legend(loc = 'upper left')


        # 误差的迹
        axs[0].plot(tr_P_posterior, "-", color="blue", label="误差的迹", linewidth="3")
        axs[0].legend(loc='upper left')
        axs[0].set_title("误差的迹", font2)


        plt.xticks(fontsize=18, fontfamily='serif')
        plt.yticks(fontsize=18, fontfamily='serif')
        plt.show()

2.3 结果图分析

在当前的参数设置的情况下，可以看到真实轨迹图（蓝色）大体上符合偏航角为60°的匀加速直线运动的。灰色线条作为测量数据，偏离程度较大；绿色线条作为建模值，偏离程度较小；最后的滤波结果则是更为接近真实值，而且误差的迹也是收敛至最小值了。

然后又研究了一下速度的真实值、先验估计值、后验估计值。

当偏航角是60°时：

当偏航角是30°时：

这说明不同偏航角情况下，相当于对不同方向的加权不一样，导致受到误差的波动影响不一样。

—————————————–以下开始非线性化模型———————————————————-

3.CTRV模型（Constant Turn Rate and Velocity 恒定转弯率和速度模型）

首先要搞清一个点：运动模型重点在于怎么建立好状态变量空间以及其状态转移函数，但是不能把运动模型和滤波算法完全混为一谈。

CTRV模型实际上是CV模型的一般形式，当角速度w=0时（需要用到洛必达法则），CTRV模型就退化成CV模型。CTRV模型假设对象沿直线前进，并且以固定的转弯速率和恒定的速度大小移动，可以看做是假设前方目标绕空间的某个点做匀加速圆周运动。

3.1 CTRV模型建模

这个过程稍微有点复杂，我自己是觉得可以直接掌握结论，因为这个模型已经很经典了。

状态量为：v指的是速度大小

状态方程为：其中速度v是常量，角速度w也是常量，与时间无关。

结合过程噪声，完整的状态方程：其中 $\LARGE a_{v,k}$ 是径向速度v的加速度， $\LARGE a_{w,k}$ 是偏航角加速度w的加速度。

在对CTRV的运动模型建模后，就相当于我们之前在卡尔曼滤波的时候，对运动方程建好模型了，那么还需要表达出测量过程。

而且还有一个问题：卡尔曼滤波仅仅用于处理线性问题，而CTRV模型是非线性的，这个时候就不能简单的使用卡尔曼滤波进行预测和更新了，此时预测的第一步变成了如下的非线性函数：

$\LARGE X_{k+1}=g\left ( X_{k},u \right )$

其中，g()表示CTRV运动模型的状态转移函数，u表示建模噪声。

为了解决非线性系统的问题，引入扩展卡尔曼滤波EKF.

3.2 扩展卡尔曼滤波EKF

先来看扩展卡尔曼滤波的整个过程：

解决非线性的问题，例如后面测量会用到的毫米波雷达，是极坐标系的雷达，像这种雷达观测到的是径向距离和角度，这时候的观测矩阵H是非线性的函数（极坐标系可以转换成笛卡尔坐标系）。

KF和EKF的区别主要就是状态转移模型（EKF用 $J_{A}$ 来解决）和观测模型（EKF用 $J_{H}$ 来解决）可以是非线性的，解决非线性的问题可以使用泰勒展开式替换成线性函数。

3.2.1 EKF的七个核心公式

（1）建模方程和观测方程

建模方程：（Motion Model）

根据之前的推导，可以知道:

很显然，g(x(t)，u)是一个多元函数，在CTRV模型之中是：（角速度等于0的时候同理）

建模过程的噪声w(t)在CTRV模型之中的表达式：

建模过程的噪声w(t)的协方差矩阵Q：

其实在之前的KF之中，实际跑代码的时候，一般都是自己手动去设置噪声方差，但是也是可以去推导公式的。

CTRV模型之中，噪声主要来自于 $\LARGE a_{v,k}$ （径向速度v的加速度）， $\LARGE a_{w,k}$ （偏航角加速度w的加速度），假设它们都符合高斯分布：

则有：

噪声的协方差矩阵为：

最后的结果：

测量方程：（Measurement Model）

参考资料：扩展卡尔曼滤波EKF (baidu.com)

在建模方程和测量方程这个部分，对我来说最难理解的反而是测量方程这个地方，因为在这里即将首次遇到非线性的测量雷达——毫米波雷达。

假设我们有激光雷达和雷达两个传感器，它们分别以一定的频率来测量以下数据：

$\bigstar$ 激光雷达：测量目标车辆的坐标（x,y）。需要注意的是，这里的x,y是相对于我们的车辆的坐标系，即我们的车辆为坐标系的原点，我们的车头为x轴，车的左侧为y轴。

$\bigstar$ 毫米波雷达：测量目标车辆在我们车辆坐标系下与本车的距离 $\rho$ ，目标车辆与x轴的夹角 $\psi$ ,以及目标车辆与我们的相对距离变化率 $\rho \dot{}$ （本质上是目标车辆的实际速度在我们和目标车辆的连线上的分量）。

在之前线性的卡尔曼滤波器中，我们使用一个测量矩阵H来把先验预测结果映射测量空间，也就是在后验估计那里，把先验估计的值 $\hat{X_{k}^{-}}$ 映射到了测量空间（就是测量值Z减去H* $\hat{X_{k}^{-}}$ ）。

卡尔曼滤波（Kalman Filtering）——（7）扩展卡尔曼滤波（EKF）一阶滤波_@曾记否的博客-CSDN博客_ekf卡尔曼滤波

我们之所以能直接这样做，是因为这个映射本身就是线性的。

而在扩展卡尔曼滤波之中，我们使用激光雷达和毫米波雷达来测量目标车辆（这个过程可以称之为传感器融合），这个时候会有两种情况：

①激光雷达[Lidar]的测量模型仍然是线性的，其测量矩阵为：

将先验预测值映射到激光雷达的测量空间：

类似于卡尔曼滤波之中的的 H* $\hat{X_{k}^{-}}$

EKF将预测映射到激光雷达的测量空间：

即：激光雷达的从状态空间到激光雷达测量空间的转换为

其中，

②将预测映射到毫米波雷达[Radar]的测量空间却是非线性的：

激光雷达测量的原理是光的直线传播，所以测量的时候可以直接获得障碍物在笛卡尔坐标系下x轴、y轴和z轴上的距离。

而毫米波雷达的原理是多普勒效应，它所测量的数据都是在极坐标系下的，而从极坐标映射到笛卡尔直角坐标系的过程并非线性过程。

毫米波雷达能够测量障碍物在极坐标下离雷达的距离ρ、方向角ϕ以及距离的变化率（径向速度）ρ'，如下图所示。

与建模过程的多元函数g(x,u)类似，测量过程中由毫米波雷达带来的非线性映射我们使用 $h(x{}')$ 来表示。它指定先验预测的位置和速度如何映射到范围，方位和范围速率的极坐标。

即：毫米波雷达的从状态空间到激光雷达测量空间的转换为

其中：

③问题所在：这两种情况怎么融合呢？

其实是我自己在编程的时候，因为要先建立测量方程嘛，所以做到这里的时候实际上就不知道测量方程要怎么写了。

然后我在这篇文章无人驾驶汽车系统入门（二）——高级运动模型和扩展卡尔曼滤波_AdamShan的博客-CSDN博客的讨论区看到了类似的问题：——既然有两组数据，数据之间怎么融合???

从评论区的讨论里面可以看出：这篇文章里面对于用于测量的两种雷达（线性的Lidar & 非线性的Radar）的数据，是作为不同时刻的数据交替采用的。

1、Lidar和Radar采集频率不一样，几乎不会同时收到数据。谁先到，谁先更新。2、不能直接组合两类传感器数据，数据间有数值矛盾，肯定发散。3、lidar数据通常需要经过分割聚类等操作，才能送到卡尔曼滤波器进行测量更新。这个过程要比radar慢些。

然后又参考了一下：KF、EKF 和 UKF 的传感器融合，用于 CV 和 CTRV 过程模型以及激光雷达和雷达测量模型

但是这篇的源代码用的是C++写的，我看不懂。事实上感觉做雷达/车辆模型很多都是用C++。

这里挖个坑：等写完后面的公式再过来把这部分的融合写完。

（2）扩展卡尔曼滤波的迭代方程

① 初始化

初始化EKF滤波器时需要输入一个初始的状态量X，用来表示目标车辆最初的位置和速度信息，一般直接使用第一次的测量结果，或者仿真的时候直接假设一下。

②先验预测（Prediction）

在（1）建模方程和观测方程中，我们建立的方程就是真实值的表达式，其中建模噪声是有表达式的，而测量噪声一般直接认为各个分量的测量噪声符合高斯分布就可以。

现在我们来研究先验预测值的迭代公式：以下提到的F就是A矩阵。

卡尔曼滤波KF的迭代公式STEP1:

但是在CTRV之中，过程模型（process model）是非线性的： $X{}'=g(x,u)$

如果将高斯分布作为输入，输入到一个非线性函数中，得到的结果将不再符合高斯分布，也就将导致卡尔曼滤波器的公式不再适用。因此我们需要将上面的非线性函数转化为近似的线性函数求解。扩展卡尔曼滤波（EKF）的思路是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。

EKF通过泰勒级数展开，省略高阶项得到一个近似的线性系统。EKF只取到一阶导，同样也能有较好的结果。取一阶导时，状态转移方程和观测方程就近似为线性方程，高斯分布的变量经过线性变换之后仍然是高斯分布，这样就能够延用标准卡尔曼滤波的框架。

具体而言，EKF对于一般的非线性函数就是这样进行一阶展开的：

回到CTRV模型之中，建模过程我们已经得到了一个多元函数 X=g（x,u）

即：（w=0的时候见上文）

那么对于这个多元函数，就需要使用多元泰勒级数：

其中，Df(a)叫雅可比矩阵，它是多元函数中各个因变量关于各个自变量的一阶偏导，一般记作J。

那么在CTRV模型之中，由于x-u的数值很小，所以可以直接忽略二阶及更高阶，而只用考虑一阶偏导，所以只需要考虑雅克比矩阵。

在CTRV模型中, 雅克比矩阵为：

算出来为：当角速度w不等于0时：

当角速度w=0时：

ps:在实际编程的时候可以直接调用函数计算雅克比矩阵。

在得到A矩阵的一阶偏导雅克比矩阵JA之后，就得到了EKF的预测部分的两个公式：

其中，处理噪声的协方差Q在上面已经推导过为:

③测量更新（Measurement Update）

下面我们将基于KF的测量更新的五个公式，推导出EKF的测量更新的三个公式。

首先看第一个，计算测量值z和先验预测值x'的差值的公式，在处理类似毫米波雷达这种非线性的模型时，习惯将计算差值的公式写成：

对应上面的式子，可以知道：

类似于g(x,u),显然h(x')也是一个非线性函数，我们也要利用泰勒一阶展开对其进行线性化，也是通过引入雅克比矩阵 $J_{H}$ 。卡尔曼运动模型公式推导CTRV+CTRA_GoodluckTian的博客-CSDN博客_ctra模型

而在处理激光雷达这种线性的测量系统时，不需要用雅克比矩阵，直接用H：

写到这里，我终于发现了一个之前被我忽略的盲点：

当使用的测量雷达不一样的时候，测量矩阵H（或者说JH）的矩阵大小不一样 !!!

从而导致了测量噪声协方差矩阵R的大小也不一样！！

当使用线性的激光雷达时：

噪声协方差矩阵P	5*5
状态转移矩阵 $J_{A}$	5*5
测量矩阵H	2*5
卡尔曼增益K	5*2
测量值z	2*1
先验预测值h(x')	2*1
测量噪声的协方差矩阵R	2*2

当使用非线性的毫米波雷达时：

噪声协方差矩阵P	5*5
状态转移矩阵 $J_{A}$	5*5
测量矩阵 $J_{H}$	3*5
卡尔曼增益K	5*3
测量值z	3*1
先验预测值h(x')	3*1
测量噪声的协方差矩阵R	3*3

那么测量更新的过程就变为：

最后总结一下各个过程及其表达式

我发现在这一些参考资料里面，其实都忽略了先验、后验这个概念，但是写代码的时候还是挺重要的，所以自己要注意！

（1）建模

过程建模：（就是真实值！加了噪声那种！）

测量建模：（就是真实值！加了噪声那种！）

分两种情况——线性的激光雷达和非线性的毫米波雷达。

线性的激光雷达：注意Zk的大小是2*1,w是随机的测量噪声

非线性的毫米波雷达：注意Zk的大小是3*1

（2）预测过程

先验估计表达式：注意先验估计是没有加噪声的啊啊啊啊啊啊啊（而且在画仿真图的时候要注意，真实轨迹应该选这种无噪的情况）

具体到CTRV模型之中：

先验误差协方差矩阵P表达式：

具体到CTRV模型之中：

当偏航角速度w不等于0的时候：

建模噪声的协方差矩阵Q:

式中,

最后结果：

（3）更新过程

此处分两种情况：使用激光雷达测量（测量矩阵H为线性）和使用毫米波雷达测量（使用非线性函数h(x'）来映射），这两种情况我们分开来讨论，并且目前我看到的是说，这两种数据并不能很好的来做同时刻的融合，很有可能是在不同时间取不同的雷达数据。

当使用激光雷达（线性）时：

卡尔曼增益表达式：

式中，测量矩阵H:

测量噪声协方差矩阵R一般就直接取一个常数矩阵:

后验估计表达式：

其中，在建模过程中已经建模了：（时间下标我没特别注意，反正k+1配k k配k-1）

这里需要注意的是，后验估计里面的zk的表达式里面是要包含测量噪声的。

误差协方差更新表达式：

当使用毫米波雷达（非线性）时：

卡尔曼增益表达式：

仍然为：

但是这里面的H矩阵和R矩阵的大小及意义都发生了改变。

由于毫米波雷达的预测映射到测量空间是非线性的，其表达式为：

类似于建模过程的g(x,u)，由于h(x')也是非线性的，所以EKF继续在此处引入h(x)的雅克比矩阵JH，得到此时的卡尔曼增益表达式：

其中：（实际写代码的时候可以调模块直接求雅克比矩阵）

此时，测量噪声的协方差矩阵R变为：（大小变成3*3了）

后验估计表达式：

式中：

以及：

误差协方差更新表达式：

那么写到这里，就有一个绕不过去的话题：

怎么融合这两种雷达的测量数据呢？这个问题我们放到3.3源代码里面看。

3.3 EKF的源代码（Python）

3.1 仅考虑线性雷达测量的EKF IN CTRV（且角速度不为0的情况）

# 该代码参考自：https://github.com/raghuramshankar/kalman-filter-localization
# 该代码使用扩展卡尔曼滤波EKF处理非线性运动模型CTRV

# 状态空间为state matrix: x方向坐标，y方向坐标，偏航角，速度，角速度(5*1)
# 输入空间为input matrix: None
# 测量矩阵为measurement matrix:从GPS处得到的x方向坐标，y方向坐标，速度和角速度(4*1)
# 本例之中是没有采用非线性雷达的 用的是线性的雷达 所以没有出现JH

import math
import numpy as np
from matplotlib import pyplot as plt
from scipy.linalg import sqrtm
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
import numdifftools as nd

# 生成正态分布的变量
def Gaussian_variable_generator(sigma):
    y = np.random.normal(loc=0.0,scale=sigma,size=None)
    return np.array(y)

#--------------------建立变量----------------------------
# 设时间间隔为t=0.1s
dt = 0.1
# 设样本数为N，可以通过键盘输入获得
N = int(input('输入样本数量N: '))

# 测量噪声的方差(4*4的矩阵)
z_noise = np.array([[0.3,0.0,0.0,0.0],
                    [0.0,0.3,0.0,0.0],
                    [0.0,0.0,0.1,0.0],
                    [0.0,0.0,0.0,0.1]])
print(z_noise.shape)

# 先验估计值的初始值(5*1的矩阵)
x_0 = np.array([[0.0],        # x方向坐标的初始值 单位：m
                [0.0],        # y方向坐标的初始值 单位：m
                [0.0],        # 偏航角yaw的初始值 单位：rad
                [1.0],        # 速度v的初始值 单位：m/s
                [0.1]])       # 角速度 yaw rate的初始值 单位：rad/s
print(x_0.shape)

# 误差P的协方差矩阵的初始值
p_0 = np.array([[0.1, 0.0, 0.0, 0.0, 0.0],
                [0.0, 0.1, 0.0, 0.0, 0.0],
                [0.0, 0.0, 0.1, 0.0, 0.0],
                [0.0, 0.0, 0.0, 0.1, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.1]])

# 过程噪声v的协方差矩阵Q
# 由于角速度以及偏航角是用的rad弧度制 所以我们可以直接用pi表达 也可以用函数np.deg2rad(45) 把45度转换成pi/4
q = np.array([[0.00001, 0.0, 0.0, 0.0, 0.0],
              [0.0, 0.00001, 0.0, 0.0, 0.0],
              [0.0, 0.0, np.deg2rad(0.000001), 0.0, 0.0],
              [0.0, 0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 0.0, np.deg2rad(1.0)]])

# 测量矩阵h
# 开头说过 本例代码会从GPS处获取x坐标、y坐标、还会测速度和角速度,所以h的大小是4*5
# 根据状态空间变量的顺序，注意h的第三行是第四个元素赋1
h = np.array([[1.0, 0.0, 0.0, 0.0, 0.0],
              [0.0, 1.0, 0.0, 0.0, 0.0],
              [0.0, 0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 0.0, 1.0]])

# 测量噪声的协方差矩阵R
# 协方差矩阵的对角线上的数字就是该变量的方差
r = np.array([[0.015, 0.0,   0.0, 0.0],
              [0.0,   0.010, 0.0, 0.0],
              [0.0,   0.0,   0.1, 0.0],
              [0.0,   0.0,   0.0, 0.01]])**2

# 状态变量
x = []

# 生成地面的实测值gz, 以及含噪声的测量值z
# 其实从现实意义上来说 含噪声的才是真实值
# 测量值的建模过程
def gen_measurement(x_true):
    # x坐标 单位m, y坐标 单位m
    gz = h @ x_true

    # 生成含高斯白噪声的测量值z 矩阵大小为4*1
    z = gz + z_noise @ np.random.randn(4,1)
    # print('z的大小',z.shape)
    return z,gz

# -----------------扩展卡尔曼滤波非线性预测步骤 利用雅克比矩阵将非线性的过程函数线性化----------------
def extended_prediction(x,p):

    # g(x) 或者说 f(x)
    # 状态空间为state matrix: x方向坐标，y方向坐标，偏航角，速度，角速度(5*1)
    # 根据积分 求出X_k 和 X_k-1之间的关系如下：
    x[0] = x[0] + x[3]/x[4] * (np.sin(x[4] * dt + x[2]) - np.sin(x[2]))
    x[1] = x[1] + x[3]/x[4] * (-np.cos(x[4] * dt + x[2]) + np.cos(x[2]))
    x[2] = x[2] + x[4] * dt
    x[3] = x[3]
    x[4] = x[4]

# 表达一下雅克比矩阵JF里面那些比较复杂的地方
# 这里之后可以去找一下有没有更加方便的表达方法，这里我就直接写出来的
# 因为我引入了symengine包去计算偏导，但是得到的结果好像传参传不进去的感觉

    a13 = float((x[3]/x[4]) * (np.cos(x[4]*dt + x[2]) - np.cos(x[2])))
    a14 = float((1.0/x[4]) * (np.sin(x[4]*dt + x[2]) - np.sin(x[2])))
    a15 = float((dt*x[3]/x[4]) * np.cos(x[4]*dt + x[2]) -
                # 两个乘号表示的是幂运算 即x[4]的平方
                (x[3]/x[4]**2) * (np.sin(x[4]*dt +x[2]) - np.sin(x[2])))

    a23 = float((x[3]/x[4]) * (np.sin(x[4]*dt +x[2]) - np.sin(x[2])))
    a24 = float(
        (1.0/x[4]) * (-np.cos(x[4]*dt +x[2]) + np.cos(x[2]))
    )
    a25 = float(
        (dt*x[3]/x[4]) * np.sin(x[4]*dt+ x[2]) -
        (x[3]/x[4]**2) * (-np.cos(x[4]*dt +x[2]) + np.cos(x[2]))
    )

# 表达出状态转移雅克比矩阵JA(或者叫JF)
    jF = np.matrix([[1.0, 0.0, a13, a14, a15],
                    [0.0, 1.0, a23, a24, a25],
                    [0.0, 0.0, 1.0, 0.0, dt],
                    [0.0, 0.0, 0.0, 1.0, 0.0],
                    [0.0, 0.0, 0.0, 0.0, 1.0]])

#   x的先验预测值 x_pred = g(x,u)
#   即EKF的第一步

    x_pred = x

#   先验误差协方差矩阵 p_pred = jf*p*jf.T + Q
#   即EKF的第二步

    p_pred = jF @ p @ np.transpose(jF) + q

    return x_pred.astype(float), p_pred.astype(float)

#---------------------------- 扩展卡尔曼滤波线性更新过程 -----------------------------
# 更新过程的输入就是预测过程的输出，即更新过程的输入是先验预测值
def linear_update(x_pred, p_pred, z):
    # 卡尔曼增益k
    # 注意不管是线性雷达还是非线性雷达 卡尔曼增益的表达式都是一样的 只是一个是H 一个是JH
    s = h @ p_pred @ np.transpose(h) + r
    k = p_pred @ np.transpose(h) @ np.linalg.pinv(s)

    # x的后验估计值 x_upd
    v = z - h @ x_pred
    x_upd = x_pred + k @ v

    # 误差的协方差更新 p_upd
    # p_upd = p_pred - k @ h @ p_pred
    p_upd = p_pred - k @ s @ np.transpose(k)
    return x_upd.astype(float), p_upd.astype(float)

#--------------------------EKF的迭代函数-----------------------------------------------------
def extended_kalman_filter(x_est, p_est, z):
    # 预测过程中输入的x_est p_est就是上一时刻更新过程中得到的后验估计值
    x_pred, p_pred = extended_prediction(x_est,p_est)
    # 更新过程中输入的x_pred p_pred就是本时刻预测过程中得到的先验估计值
    x_upd, p_upd = linear_update(x_pred,p_pred,z)

    # 最后EKF输出的就是后验估计值 x_upd p_upd
    return x_upd, p_upd


#----------------------------------------主程序-------------------------------------------
def main():
    show_final = int(input('是否显示最终结果？(是/否 = 1/0) : '))
    show_animation = int(input('是否显示滤波器的处理过程动画？(是/否 = 1/0) : '))
    if show_animation == 1:
        show_ellipse = int(
            input('是否在动画中显示协方差椭圆？(是/否 = 1/0) : ')
        )
    else:
        show_ellipse = 0

#----------对各个变量的初始值进行赋值---------------
#  函数extended_kalman_filter的输入是上一时刻的后验估计值x_est,p_est 输出的是当前时刻的后验估计值x_upd,p_upd
#  初始化的时候，直接取x_0作为初值
    x_est = x_0
    p_est = p_0

    #x_true是真实、无噪的轨迹曲线
    x_true = x_0
    p_true = p_0

    # [x_0[0, 0], x_0[1, 0]分别是x方向的坐标和y方向的坐标
    x_true_cat = np.array([x_0[0,0], x_0[1,0]])
    x_est_cat = np.array([x_0[0,0], x_0[1,0]])
    z_cat = np.array([x_0[0,0], x_0[1,0]])
    p_est_cat = np.array([p_0[0,0]+p_0[1,1]+p_0[2,2]+p_0[3,3]+p_0[4,4]])

    for i in range(N):
        x_true, p_true = extended_prediction(x_true, p_true)
        # 经过 extended_prediction 输出的x_true是不含噪声的 p_true是加了过程噪声的

        # 生成真实测量值gz(不含噪声) 以及包含噪声的测量值z
        z, gz = gen_measurement(x_true)

        # 控制循环迭代是否停止
        if i == (N - 1) and show_final == 1:
            show_final_flag = 1
        else:
            show_final_flag = 0

        # 控制是否显示动画的函数
        postpross(i, x_true, x_true_cat, x_est, p_est, x_est_cat, z,
                  z_cat, show_animation, show_ellipse, show_final_flag)

        # 最后的后验估计值
        x_est, p_est = extended_kalman_filter(x_est, p_est, z)

        # np.vstack(): 在竖直方向上堆叠，从而达到拼接数组的作用
        # 每次进行一次循环，就会产生一组坐标的真实值，所以要利用np.vstack()函数把x_true的前两个变量拼成一个数组
        # x_true_cat 的作用好像就是用来记录 + 拼接的
        x_true_cat = np.vstack((x_true_cat, np.transpose(x_true[0:2])))
        z_cat = np.vstack((z_cat, np.transpose(z[0:2])))
        x_est_cat = np.vstack((x_est_cat, np.transpose(x_est[0:2])))
        #     我自己加的，想看一下误差的协方差矩阵的迹
        p_est_cat = np.vstack(
            (p_est_cat, (p_est[0, 0] + p_est[1, 1] + p_est[2, 2] + p_est[3, 3] + p_est[4, 4]))
        )

    print('EKF Over')
    plt.plot(p_est_cat,'b', label='误差的协方差的迹')
    plt.title('误差的协方差的迹')
    plt.show()


# ---------------------------画图模块----------------------------------------
# 动画模块
def plot_animation(i, x_true_cat, x_est_cat, z):
    # i=0 即不展示动画过程
    if i == 0:
        # 真实轨迹是红色点图
        plt.plot(x_true_cat[0], x_true_cat[1], '.r')
        # 滤波输出的结果(后验估计) 是蓝色点图
        plt.plot(x_est_cat[0], x_est_cat[1], '.b')
    else:
        # 遍历x_true_cat、x_est_cat 的x坐标和y坐标
        plt.plot(x_true_cat[0:, 0], x_true_cat[0:, 1], 'r')
        plt.plot(x_est_cat[0:, 0], x_est_cat[0:, 1], 'b')
    # 含噪声的测量值的轨迹是绿色的加号
    plt.plot(z[0], z[1], '+g')
    plt.grid(True)
    plt.pause(0.001)

# 画椭圆的模块
def plot_ellipse(x_est, p_est):
    # 在0~2pi之间取100个点
    phi = np.linspace(0, 2 * math.pi, 100)
    # 把误差的协方差矩阵的x、y坐标的部分提取出来了
    p_ellipse = np.array(
      [[p_est[0, 0], p_est[0, 1]],[p_est[1,0],p_est[1,1]]]
    )
    # sqrtm是矩阵开平方,即P=A*A,则sqrtm(P)=A
    x0 = 3 * sqrtm(p_ellipse)
    xy_1 = np.array([])
    xy_2 = np.array([])
    for i in range(100):
        arr = np.array([[math.sin(phi[i])],[math.cos(phi[i])]])
        arr = x0 @ arr
        # np.hstack(): 在水平方向上平铺
        xy_1 = np.hstack([xy_1, arr[0]])
        xy_2 = np.hstack([xy_2, arr[1]])
    plt.plot(xy_1 + x_est[0], xy_2 + x_est[1], 'y')
    plt.pause(0.00001)


def plot_final(x_true_cat, x_est_cat, z_cat):
#   x_true_cat : 真实无噪的运动轨迹
#   x_est_cat : EKF输出的后验估计值画出的运动轨迹
#   z_cat : 测量值
    fig = plt.figure()
    # 增加子图的函数，三个数字分别表示x,y,z
    f = fig.add_subplot(1,1,1)
    f.plot(x_true_cat[0:, 0], x_true_cat[0:, 1], 'r', label='真实位置')
    f.plot(x_est_cat[0:, 0], x_true_cat[0:, 1], 'b', label='后验估计位置')
    f.plot(z_cat[0:, 0], z_cat[0:, 1], '.g', label='含噪的测量值')
    f.set_xlabel('x[m]')
    f.set_ylabel('y[m]')
    f.set_title('Extended Kalman Filter - CTRV模型')
    f.legend(loc= 'upper left', shadow = True, fontsize = 'large')
    plt.grid(True)
    plt.show()

def postpross(i, x_true, x_true_cat, x_est, p_est, x_est_cat, z, z_cat, show_animation, show_ellipse, show_final_flag):
    # 如果要展示动画效果
    if show_animation == 1:
        plot_animation(i, x_true_cat, x_est_cat, z)
        if show_ellipse == 1:
            plot_ellipse(x_est[0:2], p_est)
    if show_final_flag == 1:
        plot_final(x_true_cat, x_est_cat, z_cat)

if __name__ == '__main__':
    main()

仿真结果图：

当样本数为200点时：