State Estimation Using Time-Varying Kalman Filter MATLAB_help文档DeepSeek翻译

shallwegzgdj

150人浏览 · 2026-03-05 00:08:29

shallwegzgdj · 2026-03-05 00:08:29 发布

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

本示例使用：

Control System Toolbox
Simulink

简介

您希望估计地面车辆在东西和南北方向上的位置和速度。车辆可以在二维空间中自由移动，不受任何约束。您设计了一个多用途导航和跟踪系统，可用于任何物体，而不仅仅是车辆。

车辆示意图

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

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

open_system(‘ctrlKalmanNavigationExample’);

Simulink 模型

车辆模型

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

$\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) \; & \text{东向位置} \; [m] \\ x_n(t) \; & \text{北向位置} \; [m] \\ s(t) \; & \text{速度} \; [m/s] \\ \theta(t) \; & \text{相对于东向的朝向} \; [deg] \\ \end{array}$

车辆参数为：

$\begin{array} {ll} P=100000 \; & \text{峰值发动机功率} \; [W] \\ A=1 \; & \text{迎风面积} \; [m^2] \\ C_d=0.3 \; & \text{阻力系数} \; [无量纲] \\ m=1250 \; & \text{车辆质量} \; [kg] \\ L=2.5 \; & \text{轴距长度} \; [m] \\ \end{array}$

控制输入为：

$\begin{array} {ll} u_T(t) \; & \text{油门位置，范围在 -1 和 1 之间} \; [无量纲] \\ u_\psi(t) \; & \text{转向角} \; [deg] \\ \end{array}$

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

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

卡尔曼滤波器设计

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

$x^[n]=[x^e[n]x^n[n]x˙^e[n]x˙^n[n]] \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]$

其中

$x^e[n] 东向位置估计 [m]x^n[n] 北向位置估计 [m]x˙^e[n] 东向速度估计 [m/s]x˙^n[n] 北向速度估计 [m/s] \begin{array} {ll} \hat{x}_e[n] \; & \text{东向位置估计} \; [m] \\ \hat{x}_n[n] \; & \text{北向位置估计} \; [m] \\ \hat{\dot{x}}_e[n] \; & \text{东向速度估计} \; [m/s] \\ \hat{\dot{x}}_n[n] \; & \text{北向速度估计} \; [m/s] \\ \end{array}$

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

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

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

$\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]$

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

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

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

A 和 G 的第三行将东向速度建模为随机游走： $x˙^e[n+1]=x˙^e[n]+w1[n]\hat{\dot{x}}_e[n+1]=\hat{\dot{x}}_e[n]+w_1[n]$ 。实际上，位置是一个连续时间变量，是速度对时间的积分 $ddtx^e=x˙^e\frac{d}{dt}\hat{x}_e=\hat{\dot{x}}_e$ 。A 和 G 的第一行表示对此运动学关系的离散近似： $(x^e[n+1]−x^e[n])/Ts=(x˙^e[n+1]+x˙^e[n])/2(\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）以 1Hz 的采样率提供这些测量值。测量噪声 $v$ 的方差，即 R 矩阵，指定为 $R = 50$ 。由于 R 被指定为标量，卡尔曼滤波器模块假定矩阵 R 是对角矩阵，其对角线元素为 50，并且维度与 y 兼容。如果测量噪声是高斯噪声，R=50 对应于 68% 的位置测量值在东西和南北方向上的实际位置的 $m\pm\sqrt{50}\;m$ 范围内。然而，卡尔曼滤波器并不需要此假设。

$w$ 的元素捕获了车辆速度在一个采样时间 Ts 内可以变化多少。过程噪声 w 的方差，即 Q 矩阵，被选择为时变的。它捕捉了这样一种直觉：当速度较大时， $w [n]$ 的典型值较小。例如，从 0 加速到 10m/s 比从 10 加速到 20m/s 更容易。具体来说，您使用估计的北向和东向速度以及一个饱和函数来构造 Q[n]：

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

$Q[n]=[1+250fsat(x˙^e2)001+250fsat(x˙^n2)] 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-25m/s 加速时间数据进行最小二乘拟合得到的。请注意，对角 Q 的选择代表了一种朴素的假设，即北向和东向的速度变化是不相关的。

卡尔曼滤波器模块输入和设置

‘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=0s 时，对位置和速度估计的初始猜测为 0。
状态估计误差协方差 P[0]： 10。假设您的初始猜测 x[0] 与其实际值之间的误差是一个标准差为 $10\sqrt{10}$ 的随机变量。
选中 使用 G 和 H 矩阵（默认 G=I 和 H=0） 复选框以指定非默认的 G 矩阵。
G： G。G 矩阵在本示例前面已定义。
H： 0。过程噪声不会影响进入卡尔曼滤波器模块的测量值 y。
取消选中 时不变 Q 复选框。Q 矩阵是时变的，并通过模块输入端口 Q 提供。由于此设置，模块使用时变卡尔曼滤波器。您可以选择此选项以使用时不变卡尔曼滤波器。对于此问题，时不变卡尔曼滤波器性能稍差，但设计更简单且计算成本更低。
R： R。这是测量噪声 $v [n]$ 的协方差。R 矩阵在本示例前面已定义。
N： 0。假设过程噪声和测量噪声之间没有相关性。
采样时间（-1 表示继承）： Ts，在本示例前面已定义。

卡尔曼滤波器模型参数

结果

通过模拟车辆执行以下机动的场景来测试卡尔曼滤波器的性能：

在 t = 0 时，车辆位于 $x_e(0)=0$ ， $x_n(0)=0$ 且静止。
朝东向，加速到 25m/s。在 t=50s 时减速到 5m/s。
在 t = 100s 时，转向北向并加速到 20m/s。
在 t = 200s 时，再次转向西向。加速到 25m/s。
在 t = 260s 时，减速到 15m/s 并进行恒速 180 度转弯。

仿真 Simulink 模型。绘制车辆位置的实际值、测量值和卡尔曼滤波器估计值。

sim(‘ctrlKalmanNavigationExample’);

figure;
% 绘制结果并用实线连接数据点。
plot(x(:,1),x(:,2),‘bx’,…
y(:,1),y(:,2),‘gd’,…
xhat(:,1),xhat(:,2),‘ro’,…
‘LineStyle’,‘-’);
title(‘位置’);
xlabel(‘东向 [m]’);
ylabel(‘北向 [m]’);
legend(‘实际值’,‘测量值’,‘卡尔曼滤波器估计值’,‘Location’,‘Best’);
axis tight;

位置结果

测量位置与实际位置之间的误差以及卡尔曼滤波器估计位置与实际位置之间的误差为：

% 东向位置测量误差 [m]
n_xe = y(:,1)-x(:,1);
% 北向位置测量误差 [m]
n_xn = y(:,2)-x(:,2);
% 卡尔曼滤波器东向位置误差 [m]
e_xe = xhat(:,1)-x(:,1);
% 卡尔曼滤波器北向位置误差 [m]
e_xn = xhat(:,2)-x(:,2);

figure;
% 东向位置误差
subplot(2,1,1);
plot(t,n_xe,‘g’,t,e_xe,‘r’);
ylabel(‘位置误差 - 东向 [m]’);
xlabel(‘时间 [s]’);
legend(sprintf(‘测量: %.3f’,norm(n_xe,1)/numel(n_xe)),…
sprintf(‘卡尔曼滤波: %.3f’,norm(e_xe,1)/numel(e_xe)));
axis tight;
% 北向位置误差
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),‘g’,t,xhat(:,2)-x(:,2),‘r’);
ylabel(‘位置误差 - 北向 [m]’);
xlabel(‘时间 [s]’);
legend(sprintf(‘测量: %.3f’,norm(n_xn,1)/numel(n_xn)),…
sprintf(‘卡尔曼滤波: %.3f’,norm(e_xn,1)/numel(e_xn)));
axis tight;

位置误差

图例显示了位置测量和估计误差（ $∣∣xe−x^e∣∣1||x_e-\hat{x}_e||_1$ 和 $∣∣xn−x^n∣∣1||x_n-\hat{x}_n||_1$ ）除以数据点数量后的归一化值。卡尔曼滤波器估计的误差比原始测量值减少了约 25%。

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

e_ve = xhat(:,3)-x(:,3); % [m/s] 卡尔曼滤波器东向速度误差
e_vn = xhat(:,4)-x(:,4); % [m/s] 卡尔曼滤波器北向速度误差
figure;
% 东向速度及其估计值
subplot(2,1,1);
plot(t,x(:,3),‘b’,t,xhat(:,3),‘r’);
ylabel(‘速度 - 东向 [m/s]’);
xlabel(‘时间 [s]’);
legend(‘实际值’,‘卡尔曼滤波器’,‘Location’,‘Best’);
axis tight;
subplot(2,1,2);
% 估计误差
plot(t,e_ve,‘r’);
ylabel(‘速度误差 - 东向 [m/s]’);
xlabel(‘时间 [s]’);
legend(sprintf(‘卡尔曼滤波器: %.3f’,norm(e_ve,1)/numel(e_ve)));
axis tight;

速度结果

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

卡尔曼滤波器速度估计正确地跟踪了实际速度的趋势。当车辆高速行驶时，噪声水平降低。这与 Q 矩阵的设计一致。两个大的尖峰出现在 t=50s 和 t=200s。这些分别是汽车经历突然减速和急转弯的时刻。这些时刻的速度变化远大于卡尔曼滤波器的预测（基于其 Q 矩阵输入）。经过几个时间步长后，滤波器估计值会跟上实际速度。