卡尔曼滤波器的特性及仿真

我们前一篇关于人物识别跟踪的文章《视频连续目标跟踪实现的两种方法和示例(更新)》里讲到，视频图像中物体的识别和跟踪用到了卡尔曼滤波器(KF)。这里对这个话题我们稍微对这个卡尔曼滤波器进行一个整理。

目标跟踪为什么用到卡尔曼滤波器(KF)?其实是因为我们假设了视频的连续帧中的每个物体在图像中的移动位置不会相差太多来进行的。我们在卡尔曼滤波器中设计了一个运动模式，这个运动模式可以让我们估计在下一个图像中每个被识别物体的所在位置，把每个位置和上一图像中的位置进行比较统计，就可以定个七七八八了。

KF本质上是一种用于估计动态系统状态的递归算法。它通过对系统的数学建模，辅以噪声和不确定性假设，能够从一系列不精确和噪声污染的测量数据中推断出系统的最优状态估计。其核心思想是利用先验知识和新获得的测量信息来更新对系统状态的估计，使得估计误差最小化。误差、噪声干扰，这些设定符合我们实际应用场景中的所能得到的条件。而只有当信噪比足够高，或者误差几乎可以忽略不计，并且构建的系统数学模型足够准确时，才可以对测量计算得到的结果抱有信心，否则我们就需要在不完美的现实数据中，进行综合

KF广泛用于各种应用场景，特别是在需要高精度和实时处理的领域，如导航系统、自动驾驶威廉希尔官方网站 、信号处理和控制系统等。

卡尔曼滤波器有多种类型，小编这里将只围绕标准卡尔曼滤波器(KF)和扩展卡尔曼滤波器(Extended KF - EKF)讲一点特性。

经典卡尔曼滤波器(KF)：

特点：适用于线性系统假设的随机信号处理。包含两步：预测和更新，每步都使用线性方程。

应用：常用于导航、控制系统及金融工程中

扩展卡尔曼滤波器(EKF)：

特点：扩展了标准KF，将其概念应用到非线性系统中。通过对非线性方程进行线性化处理，使用雅可比矩阵来估计状态。简单地讲，就是把分线性方程在每个取值点处进行泰勒展开

应用：常用于非线性系统中，如机器人定位、航空航天导航。

卡尔曼滤波器是一种用于估计动态系统状态的最优递归算法，其核心基于以下两组基本状态方程模型：

	线性离散状态空间方程	非线性离散状态空间方程
过程方程

观测方程

KF对应的是上面的线性离散状态方程，EKF对应的是上面的非线性离散状态空间方程。

上面提到的非线性离散状态空间方程中，噪声特性是非加性噪声。如果是加性噪声，那么对应下面的公式：

        （1）        

        （2）        

但我们完全可以把这种加性噪声作为非加性噪声的一个特例处理。后面会提到并说明。

请记住这里的表示的系统状态一般都是一个多维向量，所以我们这里讨论随机状态分布的噪声w和v两个随机噪声变量其实是期望值等于0，协方差矩阵分别为Q、R的正态分布的矩阵。其中：

：nx1状态向量，包含系统在时间步 ( k ) 的状态；

A是状态转移矩阵，定义了系统的状态如何从一个时间步转移到下一个时间步；

u是px1控制输入向量，描述了在时间步 ( k ) 作用于系统的外部控制输入；

B是控制输入矩阵，描述控制输入如何影响状态转移；

是观测向量，包含系统在时间步(k)的测量值或观测值；

H是观测矩阵，将系统状态映射到观测空间；

w通常假设为均值为零的高斯噪声，影响状态转移；

v通常假设为均值为零的高斯噪声，影响观测；

f(x,u)，状态转移函数，描述系统状态如何通过非线性函数从一个时间步转移到下一个时间步；

h(x)，控制输入函数，用于描述控制输入如何与系统状态通过非线性函数关联。

对于正态分布的信号，经过线性变换之后对应的信号仍然符合正态分布；而经过非线性变换之后的非加性噪声，一般就失去了正态分布的特性。示例如下：

一个正态分布的信号x~N(0, 1)，分别经过:

经 Y = 3x + 3 转换或者映射之后，在0点的x取值是正态分布的情况下，线性变换后的Y仍然是符合正态分布，不过期望值=3，而Y的信号的方差变成了9。

E[Y] = E[3x+3] =E[3x] + 3 =3E[x] + 3 = 3*0 + 3 = 3

D[Y]=E{[Y - E(Y)]^2} =E{[3x+3 - 3]^2} =E[9x^2] =9E[x^2]=9

经Z = sin(x) 非线性转换后，虽然其期望值（均值）仍然为0，但是分布不再符合正态分布的特性。

[图-1]正态分布经过不同变换前后得到的分布比较

大家可以用下面的代码用不同的转换进行正态分布经历不同的转换后的分布interwetten与威廉的赔率体系 。

import numpy as np
import matplotlib.pyplot as plt


# Parameters for the normal distribution
mu_x = 0      # Mean of X
sigma_x = 1   # Standard deviation of X


# Linear transformation parameters
a = 3         # Scale factor
b = 3         # Shift factor


# Generate random samples from the normal distribution (X)
x = np.random.normal(mu_x, sigma_x, 5000)


# Apply the linear and nonlinear transformations
y = a * x + b
z = np.sin(x)


# 假设 x, y, z 是你的数据，mu_x, sigma_x, a, b 是参数。


# 设置子图
fig, axes = plt.subplots(3, 1, figsize=(15, 5))  # 1 行，3 列


# 在第一个子图中绘制 X 的直方图
axes[0].hist(x, bins=1000, density=True, alpha=0.5, color='blue', label=f'X ~ N({mu_x}, {sigma_x}²)')
axes[0].set_xlabel('X')
axes[0].set_ylabel('Density')
axes[0].set_xlabel('Value')
axes[0].set_ylabel('Probability Density')
axes[0].set_title('Normal Distribution')
axes[0].legend()
# 在第二个子图中绘制 Y 的直方图
axes[1].hist(y, bins=1000, density=True, alpha=0.5, color='orange', label=f'Y = {a}X + {b}')
axes[1].set_xlabel('Y')
axes[1].set_ylabel('Density')
axes[1].set_xlabel('Value')
axes[1].set_ylabel('Probability Density')
axes[1].set_title('Distribution Of Linear Transformations')
axes[1].legend()
# 在第三个子图中绘制 Z 的直方图
axes[2].hist(z, bins=1000, density=True, alpha=0.5, color='green', label=r'Z = sin(X)')
axes[2].set_xlabel('Z')
axes[2].set_ylabel('Density')
axes[2].set_xlabel('Value')
axes[2].set_ylabel('Probability Density')
axes[2].set_title('Distribution Of Nonlinear Transformations')
axes[2].legend()
# 添加整体标题
plt.suptitle('Distributions of X, Y and Z')


# 显示图表
plt.tight_layout()
plt.show()

我们再回到EKF中，对于非线性系统的状态方程，是通过对非线性系统的泰勒一次展开的方式在小区间内线性化推导（如我们在《红外热电堆的特性浅析及应用》中的对热电堆内部结构热传递的处理，以及《NTC测温—查表计算vs公式计算》中局部的线性化处理等），然后得到的处理公式；而对于加性噪声和非加性噪声，在最后的递归处理步骤中，我们会看到不同的表达处理方式。
小编这里不对KF或者EKF最后的解算步骤进行推演，经常从卡尔曼滤波器中的两个方程结果的示意图中看到，KF解决的问题就是如何从两种手段得到的系统状态都是随机数的情况下，如何来获取更优结果。在收敛的情况下，我们通过KF或者EKF往往可以得到更优值。

[图-2]卡尔曼滤波器的状态、测量和输出噪声分布

由于过程噪声w~N(0,Q)和测量噪声v~N(0,R)的存在，让我们在根据模型得到的被噪声干扰后的状态随机值和由传感器或者其他数据采集设备得到的另外一组状态随机值之间需要做一个平衡后的输出，最后的状态值，也肯定会偏向在Q或者R中那个协方差更小的值。
有很多的书，网上也有很多的文章或者视频介绍KF和EKF的推演的过程。有关文献见本文参考部分。
必须要吐槽一下公众号无法使用“圈”外的weblink，否则就直接上网页链接，而不是拷贝链接了。

[图-3]KF计算步骤[1][3]
在KF中，状态转移矩阵A，控制输入向量u，控制输入矩阵B，观测矩阵H，测量向量Zk，过程噪声协方差Q，测量噪声协方差R，以及必要的后验估计状态值的初始值，后验估计误差协方差等都是已知的参数，就可以进行计算了：

从先验估计值开始第一步计算；

然后计算后验估计误差协方差；

计算Kalman增益；

代入测量向量Zk和卡尔曼增益，得到后验估计值(状态的中间结果)；

迭代得到后验估计误差协方差，为下一次迭代准备。  

[图-4]EKF的计算步骤(非加性噪声)[1][2][3]

在上面的EKF计算步骤中，符号名称很多还是一致的，但实际操作上存在一些差异。下面是它的操作步骤。

从先验估计值开始第一步计算；

计算雅可比矩阵：

然后计算后验估计误差协方差；

计算Kalman增益，计算该增益前，包括以下两个雅可比计算子项：

代入测量向量Zk和卡尔曼增益，得到后验估计值(状态的中间结果)；

迭代得到后验估计误差协方差，为下一次迭代准备。

如果是加性噪声呢？我们再看前面的公式(1)和(2)，这时用下面的fA和hA来替代对应的公式：

这种情况下，对和分别对w和v求偏导时候，得到的雅可比矩阵都是单位对角矩阵I，Wk和Vk就可以直接消掉了。 在EKF中，雅可比矩阵用于线性化非线性系统模型，并将其应用于状态更新和测量更新的过程。例如下面状态方程对状态变量的偏导数。

对于状态更新，雅可比矩阵是系统状态方程对状态变量的偏导数，通常这个雅可比矩阵的维度是n×n，其中n是状态变量的数量。

对于测量更新，雅可比矩阵是观测方程对状态变量的偏导数，其维度通常是m×n，其中m是测量变量的数量。如果m和n相等，那么这个情况下的雅可比矩阵也是方形的。

KF示例

在该示例中，我们用平抛物体的轨迹使用KF进行处理。按道理，因为存在1/2*g*t^2，这应该是一个非线性系统，能够用状态转移矩阵(A)建立一个线性状态模型，如x下面代码所示，那么卡尔曼滤波器（KF）就足够了。

卡尔曼滤波器适用于线性高斯状态模型和观测模型的情况。在示例的模型中，状态转移矩阵(A)是线性的，因为它具有固定的系数来描述位置、速度和加速度之间的关系，而没有非线性项。如果你的测量模型（测量方程的矩阵）也是线性的，那么你可以直接使用标准的卡尔曼滤波器解决问题。

示例中，我们给定的初始条件：

x为水平方向，初始位置=0；

vx为水平速度方向，初始速度=3m/s:

x = v0 + vx * t；

y为垂直方向，初始位置和速度都是为0

y = y0 + vy0 * t +1/2 * g * t^2；

垂直方向只有加速度g，水平方向没有阻力；

过程噪声协方差Q赋值较小：

Q = np.eye(5) * 0.02 # Process noise covariance

Q[4,4] = 0 # 1g - gravity acceleration，稳定无噪声

测量噪声协方差R赋值稍大：

R = np.eye(2) * 1.5 # Measurement noise covariance

初始的后验估计状态误差协方差P较大(我们要看一下收敛特性)：

P = np.eye(5) * 50

先看结果后看模拟代码。为了彰显KF的功能，把观测状态的协方差设置的稍微大了些，而把状态转移方程的误差协方差设置得更小一些。可以看到几乎发散的状态的测量观测值（红线），KF的后验状态估计值都更靠近状态转移方程得到的数值。

[图-5]KF处理抛物线仿真（实蓝线）

[图-6]KF处理抛物线仿真x方向的位置后验估计误差协方差的收敛

特点说明：

尽管观测测量的状态乱七八糟，但是幸亏状态转移的模型足够准确，让最终的后验状态估计，那条实蓝线，仍然可以绕在理论轨迹附近；

尽管后验估计状态误差协方差P较大（50），但是KF处理过程中，我们可以看到该协方差以非常快的速度就收敛到了一个较小且稳定的数值。代码中对标准差范围取值有补充说明。图-6中的灰色轮廓线只用到了+/-2σ的公差范围。

抛物线KF模拟代码：

import numpy as np
import matplotlib.pyplot as plt


# Define constants
dt = 0.05  # time interval
g = 9.81  # gravitational acceleration


# Initial state [x, vx, y, vy, ay]
initial_state = np.array([0, 3, 0, 0, -g])  # initial position (0,0) with vx0=3, vy0=0


# State transition matrix
F = np.array([[1, dt, 0, 0, 0],
              [0, 1, 0, 0, 0],
              [0, 0, 1, dt, 0.5 * dt**2],
              [0, 0, 0, 1, dt],
              [0, 0, 0, 0, 1]])


# Observation matrix
H = np.array([[1, 0, 0, 0, 0], 
              [0, 0, 1, 0, 0]])


# Process and measurement noise matrices
Q = np.eye(5) * 0.02    # Process noise covariance
Q[4,4] = 0              # 1g - gravity acceleration


R = np.eye(2) * 1.5     # Measurement noise covariance


# Initial estimation covariance matrix
P = np.eye(5) * 50


# Lists to store positions
posterior_state_estimate, ideal_positions, measurements = [], [], []
prior_state_estimate = []
# Time steps for simulation
n_steps = 51


# Initialize state variables
state = initial_state.copy()
ideal_state = initial_state.copy()


# 生成过程噪声，可以使用 Q 的对角线元素生成
process_noise_variance = np.diag(Q)  # 假设是一个对角矩阵


# 用于存储方差的列表
position_X_variances = []
position_X_variances.append(P[0, 0])
position_Y_variances = []
position_Y_variances.append(P[2, 2])


process_state = initial_state.copy()
z_state_measure = initial_state[[0, 2]]


for _ in range(n_steps):
    # 状态空间方程得到的模拟数据
    process_noise = np.random.normal(0, process_noise_variance, size=state.shape)
    process_state = F @ process_state + process_noise


    # 模拟测量得到的状态：measurement state
    z_state_measure = H @ process_state + np.random.normal(0, np.sqrt(R.diagonal()))


    # Prediction step
    process_noise = np.random.normal(0, process_noise_variance, size=state.shape)
    state = F @ state + process_noise
    P = F @ P @ F.T + Q


    prior_state_estimate.append((state[0], state[2]))
    # Assume we get some measurements (with noise)
    ideal_state = F @ ideal_state
    #z_state_measure = H @ ideal_state + np.random.normal(0, np.sqrt(R.diagonal()))


    # Kalman Gain
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)


    # Update step
    y = z_state_measure - H @ state
    state += K @ y
    P = (np.eye(len(K)) - K @ H) @ P


    # 存储 x，y 位置的方差
    position_X_variances.append(P[0, 0])
    position_Y_variances.append(P[2, 2])
    # Store the results
    posterior_state_estimate.append((state[0], state[2]))
    ideal_positions.append((ideal_state[0], ideal_state[2]))
    measurements.append((z_state_measure[0], z_state_measure[1]))


# Convert lists to numpy arrays
prior_state_estimate = np.array(prior_state_estimate)
posterior_state_estimate = np.array(posterior_state_estimate)
ideal_positions = np.array(ideal_positions)
measurements = np.array(measurements)


# Plot the results
plt.figure(figsize=(10, 6))
plt.plot(prior_state_estimate[:,0],prior_state_estimate[:,1], label = 'Prior State Estimate',linestyle='--')
plt.plot(ideal_positions[:, 0], ideal_positions[:, 1], label='Ideal Trajectory', linestyle='--')
#plt.scatter(measurements[:, 0], measurements[:, 1], c='orange', label='Measurements', marker='o')
plt.plot(measurements[:, 0], measurements[:, 1], c='red', label='Measurements', linestyle='-')
plt.plot(posterior_state_estimate[:, 0], posterior_state_estimate[:, 1], c='blue', label='Posterior Estimated Trajectory', linestyle='-')
plt.xlabel('x position (m)')
plt.ylabel('y position (m)')
plt.title('Projectile Motion with Kalman Filter - Ideal vs Estimated vs Measured')
plt.legend()
plt.grid()
plt.show()


# 绘制X位置的方差随时间的变化
"""
1倍标准差（±1σ）：大约68.27%的数据落在一个标准差范围内，即从分布平均值向左右各一个标准差。
2倍标准差（±2σ）：大约95.45%的数据落在两个标准差范围内，这一个常用的置信区域，许多统计分析中常被用于定义数据的正常范围。
3倍标准差（±3σ）：大约99.73%的数据落在三个标准差范围内，通常用于识别异常值或者极端值。
"""
time_steps = range(n_steps+1)
plt.figure(figsize=(10, 5))
plt.plot(time_steps, position_X_variances, label='Variance of X Position', color='blue')
# Calculate twice the standard deviation
twice_std_dev = 2 * np.sqrt(position_X_variances)
plt.fill_between(time_steps, 
                 0 - np.sqrt(twice_std_dev), 
                 0 + np.sqrt(twice_std_dev), 
                 color='gray', alpha=0.5, step='mid', label='2 Std Dev Range')
plt.xlabel('Time Step')
plt.ylabel('Variance')
plt.title('Convergence of X Position Variance')
plt.legend()
plt.grid(True)
plt.show()


# 绘制Y位置的方差随时间的变化
plt.figure(figsize=(10, 5))
plt.plot(time_steps, position_Y_variances, label='Variance of Y Position', color='blue')
# Calculate twice the standard deviation
twice_std_dev = 2 * np.sqrt(position_Y_variances)
plt.fill_between(time_steps, 
                 0 - np.sqrt(twice_std_dev), 
                 0 + np.sqrt(twice_std_dev), 
                 color='gray', alpha=0.5, step='mid', label='2 Std Dev Range')
plt.xlabel('Time Step')
plt.ylabel('Variance')
plt.title('Convergence of Y Position Variance')
plt.legend()
plt.grid(True)
plt.show()

EKF示例

该示例中，模拟一辆小车始终以沿着其自身的中轴线用速度V行驶，而且小车整体还以固定的转速转动。我们以这个为前提对其行驶轨迹[x,y]和转动角度进行跟踪输出。

[图-7]模拟小车的行驶和转动

我们可以根据条件得到以下的状态转移方程(3)和观测测量方程(4)：

(3)

(4)

其中，w~N(0, Q)和v~[0, R]分别是过程噪声和测量误差噪声。

根据离散状态方程，我们可以得到[图-4]中的和：

（5）

（6）

而[图-4]中的Wk和Vk则是两个单位矩阵。上面公式(5)和(6)需要留意代入的值。

先上图后看代码。仿真该小车逐渐转弯的行驶轨迹和转动角度的大小如下图所示。

[图-8]EKF模拟输出的轨迹和转动角度

如论如何，如果状态方程和观测方程的数值都不可靠，那么无论KF还是EKF，可能都将无法得到我们期望的结果。
EKF示例小车轨迹和转角模拟代码如下：

import numpy as np
import matplotlib.pyplot as plt


# A state space model for a differential drive mobile robot


# A matrix - 3x3 identity matrix
A_t_minus_1 = np.array([[1.0,  0,  0],
                        [  0,1.0,  0],
                        [  0,  0, 1.0]])


# Function to get the B matrix
def getB(yaw, dt):
    """
    Calculates and returns the B matrix
    3x2 matrix -> number of states x number of control inputs
    Expresses how the state of the system [x,y,yaw] changes
    due to the control commands
    :param yaw: The yaw angle in radians
    :param dt: The time change in seconds
    """
    B = np.array([[np.cos(yaw) * dt, 0],
                  [np.sin(yaw) * dt, 0],
                  [0, dt]])
    return B


# Define Jacobian for the transition function
def jacobian_of_motion(state, control, dt):
    _, _, yaw = state
    v, _ = control
    J_f = np.array([
        [1, 0, -v * np.sin(yaw) * dt],
        [0, 1, v * np.cos(yaw) * dt],
        [0, 0, 1]
    ])
    return J_f


def ekf_simulation(initial_state, control_vector, total_time, time_step):
    # Initialize state for EKF
    state_estimate = initial_state
    states_over_time = [state_estimate]


    # Simulated and ideal states tracking
    simulated_states = [initial_state]
    ideal_states = [initial_state]
    prior_states_estimate = [initial_state]
    
    # Initial posterior covariance of new estimate
    Pk = np.diag([1.0, 1.0, 1.0])


    # Measurement noise covariance matrix R_k
    # Measurement Noise init
    std_dev_x_mes = 0.5    # Standard deviation for x measurement
    std_dev_y_mes = 0.5    # Standard deviation for y measurement
    std_dev_yaw_mes = 0.15  # Standard deviation for yaw measurement
    
    R_k = np.diag([std_dev_x_mes**2, std_dev_y_mes**2, std_dev_yaw_mes**2])
    
    H = np.array([[1, 0, 0],
                  [0, 1, 0],
                  [0, 0, 1]])


    # Jacobian matrix J_h, which is the same as H for a linear model
    J_h = H
    
    # Process Noise init
    std_dev_x = 0.15
    std_dev_y = 0.15
    std_dev_yaw = 0.15


    # Simulation loop
    for _ in np.arange(0, total_time, time_step):
        # Ideal state (no noise)
        ideal_state = A_t_minus_1 @ ideal_states[-1] + getB(ideal_states[-1][2], time_step) @ control_vector
        ideal_states.append(ideal_state)


        # 1-1. State Prediction step
        # 生成过程噪声
        process_noise_sim = np.random.normal(0, [std_dev_x, std_dev_y, std_dev_yaw], size=ideal_state.shape)


        Prior_state_Estimate = (A_t_minus_1 @ states_over_time[-1] + getB(states_over_time[-1][2], time_step) @ control_vector
                                + process_noise_sim)
        prior_states_estimate.append(Prior_state_Estimate)
        # 1-2. Process Error covariance
        Jf = jacobian_of_motion(states_over_time[-1], control_vector, dt=time_step)
        
        # Generate process noise for simulation as additive noise
        process_noise = np.random.normal(0, [std_dev_x, std_dev_y, std_dev_yaw])
        # Convert to a diagonal covariance matrix if needed for use in the EKF
        process_noise_cov = np.diag(process_noise**2)   
        
        Pk = Jf @ Pk @ Jf.T +  process_noise_cov #np.random.uniform(-process_noise, process_noise, 3)     
        
        # 2-1. Compute the Kalman gain
        K_k = Pk @  J_h.T @ np.linalg.inv(J_h @ Pk @ J_h.T + R_k)


        # Simulate sensor reading with measurement noise (using ideal_state)
        #tolerance_noise = np.random.uniform(-measurement_noise, measurement_noise, 3)
        measurement_noise = np.random.normal(0, [std_dev_x_mes, std_dev_y_mes, std_dev_yaw_mes])
        simulated_state = ideal_state + measurement_noise
        
        simulated_states.append(simulated_state)
        
        # 2-2. Update estimate with measurement z_k
        state_estimate = Prior_state_Estimate + K_k @ (simulated_state - J_h @ Prior_state_Estimate)
        
        # 2-3. Update the error covariance
        Pk = (np.eye(3) - K_k @ J_h) @ Pk


        # Record the updated state
        states_over_time.append(state_estimate)


    return np.array(prior_states_estimate), np.array(states_over_time), np.array(simulated_states), np.array(ideal_states)


def plot_states(prior_states_estimate, states_over_time, simulated_states, ideal_states):
    plt.figure(figsize=(12, 8))


    # Plot position on XY plane
    plt.subplot(2, 1, 1)
    plt.plot(ideal_states[:, 0], ideal_states[:, 1], 'g-', label='Ideal Path')
    plt.plot(prior_states_estimate[:,0],prior_states_estimate[:,1], label='prior Path estimate')
    plt.plot(simulated_states[:, 0], simulated_states[:, 1], 'b--', label='Simulated Measure Path')
    plt.plot(states_over_time[:, 0], states_over_time[:, 1], 'r-', label='EKF posterior state Path')


    plt.xlabel('X Position (meters)')
    plt.ylabel('Y Position (meters)')
    plt.title('Trajectory Over Time')
    plt.legend()
    plt.grid(True)


    # Plot yaw angle over time
    plt.subplot(2, 1, 2)
    time_points = np.arange(0, len(states_over_time))
    plt.plot(time_points, ideal_states[:, 2], 'g-', label='Ideal Yaw')
    plt.plot(prior_states_estimate[:,2], label='prior Yaw estimate')
    plt.plot(time_points, simulated_states[:, 2], 'b--', label='Simulated Measure Yaw')
    plt.plot(time_points, states_over_time[:, 2], 'r-', label='EKF posterior state Yaw')


    plt.xlabel('Time Step')
    plt.ylabel('Yaw (radians)')
    plt.title('Yaw Angle Over Time')
    plt.legend()
    plt.grid(True)


    plt.tight_layout()
    plt.show()


def main():
    # Initial conditions
    initial_state = np.array([0.0, 0.0, 0.0]) # [x_init, y_init, yaw_init]
    control_vector = np.array([4.5, 0.15])    # [v, yaw_rate]


    # Driving parameters
    total_time = 10.0 # Total simulation time in seconds
    time_step = 0.05   # Time step for each iteration in seconds


    # Run simulation
    prior_states_estimate, ekf_states, simulated_states, ideal_states = ekf_simulation(
        initial_state, control_vector, total_time=total_time, time_step=time_step)


    # Plot the simulation results
    plot_states(prior_states_estimate, ekf_states, simulated_states, ideal_states)


main()

大家可以对照模拟中的代码操作不走和[图-4]及和EKF操作步骤的描述部分，看看模拟是如何进行的，是否还有没有考虑的问题。以上仿真的输出图中，我们都加入了理想状态值作为参考和比较。