使用时变卡尔曼滤波器进行状态估计

R2026b

此示例使用:

打开脚本

此示例说明如何在 Simulink® 中使用时变卡尔曼滤波器来估计线性系统的状态。您将使用 Control System Toolbox™ 库中的 Kalman Filter 模块，根据含噪位置测量值（例如 GPS 传感器测量值）来估计地面车辆的位置和速度。卡尔曼滤波器中的被控对象模型具有时变噪声特征。

简介

您需要估计地面车辆在北向和东向的位置与速度。该车辆可在二维空间内自由移动，不受任何约束。您设计了一套多用途导航与跟踪系统，该系统可用于任何对象，而不仅仅是车辆。

$x_e(t)$ 和 $x_n(t)$ 分别是车辆相对于原点的东向和北向位置，$\theta(t)$ 是车辆相对于东向的朝向，$u_\psi(t)$ 是车辆的转向角。$t$ 是连续时间变量。

该 Simulink 模型由两个主要部分组成：车辆模型和卡尔曼滤波器。以下各节将进一步说明这两个部分。

open_system("KalmanNavigationExample")

车辆模型

所跟踪的车辆用一个简单的质点模型表示：

$$ \frac{d}{dt} \left[
\begin{array} {c}
 x_e(t) \\
 x_n(t) \\
 s(t) \\
 \theta(t)
\end{array} \right] = \left[
\begin{array} {c}
 s(t)\cos(\theta(t)) \\
 s(t)\sin(\theta(t)) \\
 (P \; \frac{u_T(t)}{s(t)} - A \; C_d \; s(t)^2) / m \\
 s(t) \tan(u_\psi(t)) / L
\end{array} \right]
$$

其中车辆状态为：

$$ \begin{array} {ll}
x_e(t) \; & \textnormal{East position} \; [m] \\
x_n(t) \; & \textnormal{North position} \; [m] \\
s(t) \; & \textnormal{Speed} \; [m/s] \\
\theta(t) \; & \textnormal{Orientation from east} \; [deg] \\
\end{array} $$

车辆参数为：

$$ \begin{array} {ll}
P=100000 \; & \textnormal{Peak engine power} \; [W] \\
A=1 \; & \textnormal{Frontal area} \; [m^2] \\
C_d=0.3 \; & \textnormal{Drag coefficient} \; [Unitless] \\
m=1250 \; & \textnormal{Vehicle mass} \; [kg] \\
L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\
\end{array} $$

控制输入为：

$$ \begin{array} {ll}
u_T(t) \; & \textnormal{Throttle position in the range of -1 and 1} \; [Unitless] \\
u_\psi(t) \; & \textnormal{Steering angle} \; [deg] \\
\end{array} $$

该模型的纵向动态特性忽略了轮胎滚动阻力。模型的横向动态特性假设可以瞬时达到期望的转向角，并忽略了横摆转动惯量。

该车辆模型在 ctrlKalmanNavigationExample/Vehicle Model 子系统中实现。Simulink 模型在 ctrlKalmanNavigationExample/Speed And Orientation Tracking 子系统中包含两个 PI 控制器，用于跟踪车辆的期望朝向和速度。这使您可以指定车辆的各种工况，并测试卡尔曼滤波器的性能。

卡尔曼滤波器设计

卡尔曼滤波器是一种基于线性模型来估计感兴趣的未知变量的算法。该线性模型描述了估计的变量随时间的演化，该演化是对模型初始条件以及已知和未知模型输入的响应。在此示例中，您将估计以下参数/变量：

$$ \hat{x}[n] = \left[
 \begin{array}{c}
 \hat{x}_e[n] \\
 \hat{x}_n[n] \\
 \hat{\dot{x}}_e[n] \\
 \hat{\dot{x}}_n[n]
 \end{array}
 \right]
$$

其中

$$ \begin{array} {ll}
\hat{x}_e[n] \; & \textnormal{East position estimate} \; [m] \\
\hat{x}_n[n] \; & \textnormal{North position estimate} \; [m] \\
\hat{\dot{x}}_e[n] \; & \textnormal{East velocity estimate} \; [m/s] \\
\hat{\dot{x}}_n[n] \; & \textnormal{North velocity estimate} \; [m/s] \\
\end{array} $$

$\dot{x}$ 项表示速度，而不是导数运算符。$n$ 是离散时间索引。卡尔曼滤波器使用的模型形式如下：

$$ \begin{array} {rl}
\hat{x}[n+1] &= A\hat{x}[n] + Gw[n] \\
y[n] &= C\hat{x}[n] + v[n]
\end{array} $$

其中 $\hat{x}$ 是状态向量。$y$ 是测量值，$w$ 是过程噪声，$v$ 是测量噪声。卡尔曼滤波器假设 $w$ 和 $v$ 是均值为零的独立随机变量，已知其方差为 $E[ww^T]=Q$、$E[vv^T]=R$ 和 $E[wv^T]=N$。其中，A、G 和 C 矩阵为：

$$ A = \left[
 \begin{array}{c c c c}
 1 & 0 & T_s & 0 \\
 0 & 1 & 0 & T_s \\
 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 1
 \end{array}
 \right]
$$

$$ G = \left[
 \begin{array}{c c}
 T_s/2 & 0 \\
 0 & T_s/2 \\
 1 & 0 \\
 0 & 1
 \end{array}
 \right]
$$

$$ C = \left[
 \begin{array}{c c c c}
 1 & 0 & 0 & 0 \\
 0 & 1 & 0 & 0
 \end{array}
 \right]
$$

其中 $T_s=1\;[s]$

A 和 G 矩阵的第三行将东向速度建模为随机游走：$\hat{\dot{x}}_e[n+1]=\hat{\dot{x}}_e[n]+w_1[n]$。实际上，位置是一个连续时间变量，并且是速度对时间的积分：$\frac{d}{dt}\hat{x}_e=\hat{\dot{x}}_e$。A 和 G 矩阵的第一行表示该运动学关系的离散逼近：$(\hat{x}_e[n+1]-\hat{x}_e[n])/Ts=(\hat{\dot{x}}_e[n+1]+\hat{\dot{x}}_e[n])/2$。A 和 G 矩阵的第二行和第四行表示北向速度与位置之间的相同关系。

C 矩阵表示仅位置测量值可用。位置传感器（如 GPS）以 1 Hz 的采样率提供这些测量值。测量噪声 $v$ 的方差（即 R 矩阵）指定为 $R=50$。由于 R 矩阵被指定为标量，因此 Kalman Filter 模块会假设 R 矩阵是对角矩阵，其对角线元素为 50，且维度与 y 兼容。如果测量噪声服从高斯分布，则 R=50 意味着东向和北向的位置测量值有 68% 位于距离实际位置不超过 $\pm\sqrt{50}\;m$ 的范围内。但是，此假设对于卡尔曼滤波器而言并不是必需的。

$w$ 中的元素反映了车辆速度在一个采样时间 Ts 内可能变化的幅度。过程噪声 w 的方差（即 Q 矩阵）被选为随时间变化。这体现了以下直观认识：当速度较大时，$w[n]$ 的典型值较小。例如，从 0 米/秒加速到 10 米/秒要比从 10 米/秒加速到 20 米/秒更容易。具体而言，您可以使用估计的北向和东向速度以及一个饱和函数来构造 Q[n]：

$$f_{sat}(z)=min(max(z,25),625)$$

$$ Q[n] = \left[
 \begin{array}{ c c }
 \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_e^2)} & 0 \\
 0 & \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_n^2)}
 \end{array}
 \right]
$$

Q 矩阵的对角线元素将 w 的方差建模为与估计速度的平方成反比。饱和函数可防止 Q 矩阵变得过大或过小。系数 250 是通过对某通用车辆在 0-5、5-10、10-15、15-20 及 20-25 米/秒加速时间段内的数据进行最小二乘拟合而得出的。请注意，选择对角矩阵 Q 表示了一种朴素假设，即北向和东向的速度变化是不相关的。

Kalman Filter 模块输入和设置

Kalman Filter 模块位于 Simulink 的 Control System Toolbox 库中。该模块还包含在 System Identification Toolbox/Estimators 库中。配置该模块的参数以进行离散时间状态估计。指定以下滤波器设置参数：

  • 时域：离散时间。选择此选项以估计离散时间状态。

  • 选中使用当前测量 y[n] 来改进 xhat[n] 复选框。这可实现离散时间卡尔曼滤波器的“当前估计器”变体。此选项可提高估计准确度，对于较慢的采样时间更为有用。但是，它会增加计算开销。此外，此卡尔曼滤波器变体具有直接馈通，如果将卡尔曼滤波器用于不包含任何延迟的反馈环中（反馈环本身也具有直接馈通），则会导致代数环。代数环可能会进一步影响仿真速度。

点击选项选项卡，以设置模块的输入端口和输出端口选项：

  • 清除添加输入端口 u 复选框。被控对象模型中没有已知输入。

  • 选中输出状态估计误差协方差 Z 复选框。Z 矩阵提供了关于滤波器对状态估计的置信度的信息。

点击模型参数以指定被控对象模型和噪声特征：

  • 模型源:单个 A、B、C、D 矩阵。

  • A:A。A 矩阵已在此示例的前面部分中定义。

  • C:C。C 矩阵已在此示例的前面部分中定义。

  • 初始估计值源:对话框

  • 初始状态 x[0]: 0。这表示在 t = 0 秒时，位置和速度估计值的初始猜测值为 0。

  • 状态估计误差协方差 P[0]：10。假设初始猜测值 x[0] 与其实际值之间的误差是一个标准差为 $\sqrt{10}$ 的随机变量。

  • 选中使用 G 和 H 矩阵(默认 G=I，H=0) 复选框，以指定非默认的 G 矩阵。

  • G:G。G 矩阵已在此示例的前面部分中定义。

  • H: 0。过程噪声不影响进入 Kalman Filter 模块的测量值 y。

  • 清除时不变 Q 复选框。Q 矩阵是时变矩阵，并通过模块输入端口 Q 提供。由于此设置，模块使用时变卡尔曼滤波器。您可以选中此选项以使用时不变卡尔曼滤波器。对于此问题，时不变卡尔曼滤波器的性能稍差，但更易于设计且计算成本更低。

  • R:R。这是测量噪声 $v[n]$ 的协方差。R 矩阵已在此示例的前面部分中定义。

  • N: 0。假设过程噪声与测量噪声之间不存在相关性。

  • 采样时间(-1 表示继承):Ts，已在此示例的前面部分中定义。

结果

通过对车辆执行以下机动动作的场景进行仿真，测试卡尔曼滤波器的性能：

  • 在 t = 0 时，车辆位于 $x_e(0)=0$, $x_n(0)=0$ 处，且处于静止状态。

  • 车辆向东行驶，加速至 25 米/秒。在 t = 50 秒时，减速至 5 米/秒。

  • 在 t = 100 秒时，车辆转向北方并加速至 20 米/秒。

  • 在 t = 200 秒时，车辆再次转向西方。加速至 25 米/秒。

  • 在 t = 260 秒时，车辆减速至 15 米/秒，并以恒定速度进行 180 度转弯。

对 Simulink 模型进行仿真。绘制车辆位置的真实值、测量值以及卡尔曼滤波器估计值。

sim("KalmanNavigationExample")

figure
% Plot results and connect data points with a solid line.
plot(x(:,1),x(:,2),"bx",...
    y(:,1),y(:,2),"gd",...
    xhat(:,1),xhat(:,2),"ro",...
    LineStyle="-")
title("Position")
xlabel("East [m]")
ylabel("North [m]")
legend("Actual","Measured","Kalman filter estimate",Location="best")
axis tight

计算测量位置与实际位置之间的误差，以及卡尔曼滤波器估计值与实际位置之间的误差。

% East position measurement error [m]
n_xe = y(:,1)-x(:,1);
% North position measurement error [m]
n_xn = y(:,2)-x(:,2);
% Kalman filter east position error [m]
e_xe = xhat(:,1)-x(:,1);
% Kalman filter north position error [m]
e_xn = xhat(:,2)-x(:,2);

figure
% East Position Errors
subplot(2,1,1)
plot(t,n_xe,"g",t,e_xe,"r")
ylabel("Position Error - East [m]")
xlabel("Time [s]")
legend(sprintf("Meas: %.3f",norm(n_xe,1)/numel(n_xe)), ...
    sprintf("Kalman f.: %.3f",norm(e_xe,1)/numel(e_xe)))
axis tight
% North Position Errors
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),"g",t,xhat(:,2)-x(:,2),"r")
ylabel("Position Error - North [m]")
xlabel("Time [s]")
legend(sprintf("Meas: %.3f",norm(n_xn,1)/numel(n_xn)), ...
    sprintf("Kalman f: %.3f",norm(e_xn,1)/numel(e_xn)))
axis tight

图例显示了按数据点数量归一化后，位置测量值与估计值之间的误差（$||x_e-\hat{x}_e||_1$ 和 $||x_n-\hat{x}_n||_1$）。卡尔曼滤波器的估计值误差比原始测量值低约 25%。

下图上方的子图显示了东向的实际速度及其卡尔曼滤波器估计值。下方的子图显示了估计值误差。

e_ve = xhat(:,3)-x(:,3); % [m/s] Kalman filter east velocity error
e_vn = xhat(:,4)-x(:,4); % [m/s] Kalman filter north velocity error

figure
% Velocity in east direction and its estimate
subplot(2,1,1)
plot(t,x(:,3),"b",t,xhat(:,3),"r")
ylabel("Velocity - East [m/s]")
xlabel("Time [s]")
legend("Actual","Kalman filter",Location="best")
axis tight
subplot(2,1,2)
% Estimation error
plot(t,e_ve,"r")
ylabel("Velocity Error - East [m/s]");
xlabel("Time [s]")
legend(sprintf("Kalman filter: %.3f",norm(e_ve,1)/numel(e_ve)))
axis tight

误差图上的图例显示了按数据点数量归一化后的东向速度估计值误差 $||\dot{x}_e-\hat{\dot{x}}_e||_1$。

卡尔曼滤波器的速度估计值正确地跟踪了实际速度趋势。当车辆高速行驶时，噪声水平会降低。这与 Q 矩阵的设计相符。图中的两个显著峰值分别出现在 t = 50 秒和 t = 200 秒处。这两个时刻分别对应车辆突然减速和急转弯。在这两个时间点，速度的变化幅度远超卡尔曼滤波器基于 Q 矩阵输入生成的预测值。但经过几个时间步后，滤波器估计值便追赶上了实际速度。

总结

您在 Simulink 中使用 Kalman Filter 模块估计了车辆的位置和速度。该模型的过程噪声动态特性随时间变化。您通过对各种车辆机动动作和随机生成的测量噪声进行仿真，验证了滤波器的性能。卡尔曼滤波器改善了位置测量值，并提供了车辆的速度估计值。

另请参阅

主题