主要内容

多速率传感器非线性系统状态估计

这个例子使用了:

打开脚本

这个例子展示了如何在Simulink™中对具有多个传感器以不同采样率工作的系统执行非线性状态估计。金宝app控制系统工具箱™中的扩展卡尔曼滤波器块用于使用GPS和雷达测量来估计物体的位置和速度。

简介

工具箱有三个Simulink块用于非线性状态金宝app估计:

  • 扩展卡尔曼滤波:实现一阶离散扩展卡尔曼滤波算法。

  • 无味卡尔曼滤波:实现了离散时间无味卡尔曼滤波算法。

  • 粒子滤波:实现了一个离散粒子滤波算法。

这些块支持使用在不同采样率金宝app下工作的多个传感器进行状态估计。使用这些块的典型工作流程如下:

  1. 使用MATLAB或Simulink函数建模您的植物和传感器行为。金宝app

  2. 配置块的参数。

  3. 仿真滤波器并分析结果，以获得对滤波器性能的信心。

  4. 在硬件上部署筛选器。您可以使用Simulink Coder™软件为这些过滤器生成代码。金宝app

本例使用Extended Kalman Filter块演示此工作流的前两个步骤。中简要讨论了最后两个步骤下一个步骤部分。本例的目标是使用雷达和GPS传感器提供的噪声测量来估计物体的状态。对象的状态是它的位置和速度，在Simulink模型中表示为xTrue。金宝app

如果您对粒子滤波块感兴趣，请参阅示例“在Simulink中使用粒子滤波块进行参数和状态估计”。金宝app

目录(fullfile (matlabroot,“例子”，“控制”，“主要”）)添加示例数据open_system (“multirateEKFExample”）;

植物建模

扩展卡尔曼滤波(EKF)算法需要一个状态转换函数来描述状态从一个时间步到下一个时间步的演化。block支持以下两金宝app种函数形式:

  • 附加过程噪声:$x[k+1] = f(x[k]， u[k]) + w[k]$

  • 非加性过程噪声:$x[k+1] = f(x[k]， w[k]， u[k])$

这里f(..)是状态转换函数，x是状态，w是过程噪声。U是可选的，表示f的额外输入，例如系统输入或参数。加性噪声意味着下一个状态美元$ x [k + 1]以及过程噪声w [k]美元线性相关。如果关系是非线性的，则使用非相加形式。

函数f(…)可以是符合MATLAB Coder™限制的MATLAB函数，也可以是Simulink函数块。金宝app在创建f(…)之后，在Extended Kalman Filter块中指定函数名以及过程噪声是加性的还是非加性的。

在这个例子中，您正在跟踪一个物体在二维平面上的北面和东面的位置和速度。估计数量为:

$$ \hat{x}[k] = \left[ c \开始{数组}{}& # xA;{x} \帽子_e [k] \ \ & # xA;{x} \帽子_n”[k] \ \ & # xA;{v} \帽子_e [k] \ \ & # xA;{v} \帽子_n”[k] & # xA;结束\{数组}& # xA;\右]& # xA;\; \begin{array} {ll} \textnormal{东位置估计}\;[m] \\ \textnormal{北方位置估计}\; [m] \\ \textnormal{East velocity estimate} \; [m/s] \\ \textnormal{North velocity estimate} \; [m/s] \\ \end{array} $$

在这里k美元是离散时间指标。所使用的状态转换方程是非加性形式的$\hat{x}[k+1] = A\hat{x}[k] + Gw[k]$,在那里$ \帽子{x} $状态向量是w美元是进程噪声。筛选器假设w美元是零均值，方差已知的独立随机变量E (ww ^ T)美元．A和G矩阵是:

$$ A = \left[ \begin{数组}{c c c c} 1 & # 38岁;0 & # 38;T_s & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;T_s \ \ & # xA; 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] $$

在哪里T_s美元是采样时间。A和G的第三行将东速度建模为随机游走:$ {v} \帽子_e (k + 1) = {v} \帽子_e [k] + w_1 [k]美元．实际上，位置是一个连续时间变量，是速度除以时间的积分$ \压裂{d} {dt} {x} _e = \ \帽子帽子{v} _e美元．A和G的第一行表示这个运动学关系的离散近似:美元(\帽子{x} _e [k + 1] - \帽子{x} _e [k]) / T_s =(\帽子{v} _e [k + 1] + \帽子{v} _e [k]) / 2美元．A和G的第二行和第四行表示北方速度与位置的关系相同。这种状态转换模型是线性的，而雷达测量模型是非线性的。这种非线性需要使用非线性状态估计器，如扩展卡尔曼滤波器。

在本例中，您使用Simulink function块实现状态转换函数。金宝app为了做到这一点，

  • 添加一个金宝app仿真软件的功能块到您的模型金宝app模型/用户定义函数图书馆

  • 单击“Simulink功能”块中显示的名称。金宝app编辑函数名，并根据需要添加或删除输入和输出参数。在本例中，状态转换函数的名称为stateTransitionFcn．它有一个输出参数(xNext)和两个输入参数(x, w)。

  • 虽然在本例中不是必需的，但您可以在Simulink函数中使用来自Simulink模型其余部分的任何信号。金宝app为此，添加轮廓尺寸街区之外金宝app模型/来源图书馆。注意，这些不同于ArgIn而且ArgOut通过函数的签名设置的块(xNext = stateTransitionFcn(x, w))。

  • 在Simuli金宝appnk函数块中，使用Simulink块构造函数。

  • 属性中设置输入和输出参数x、w和xNext的尺寸信号的属性的标签。ArgIn而且ArgOut块。数据类型和端口尺寸必须与中提供的信息一致扩展卡尔曼滤波器块。

在本例中还实现了状态转移函数的解析雅可比矩阵。指定雅可比矩阵是可选的。然而，这减少了计算负担，并在大多数情况下提高了状态估计精度。将雅可比函数实现为Simulink函数，因为状态转换函数是一个Simuli金宝appnk函数。

open_system (multirateEKFExample/S金宝appimulink函数-状态转换雅可比矩阵）;

传感器建模-雷达

Extended Kalman Filter块还需要一个测量函数来描述状态与测量值之间的关系。支持以下两种函数形式:金宝app

  • 附加测量噪声:$y[k] = h(x[k]， u[k]) + v[k]$

  • 非加性测量噪声:$y[k] = h(x[k]， v[k]， u[k])$

这里h(..)是测量函数，v是测量噪声。U是可选的，表示h的额外输入，例如系统输入或参数。这些输入可以不同于状态转换函数中的输入。

在这个例子中，位于原点的雷达以20hz的频率测量物体的距离和角度。假设两个测量值都有大约5%的噪声。这可以通过以下测量方程进行建模:

$$ y_{雷达}[k] = \left[ c \开始{数组}{}& # xA;大概{x_n \ [k] ^ 2 + x_e [k] ^ 2} \;(1 + v_1 [k]) \ \ & # xA;Atan2 (x_n[k]， x_e[k]) \;(1 + v_2 [k]) \ \ & # xA;结束\{数组}& # xA;\右]& # xA; $ $

在这里v_1 [k]美元而且美元$ v_2 [k]是测量噪声项，每个方差为0.05^2。也就是说，大多数测量的误差小于5%。测量噪声是非加性的，因为v_1 [k]美元而且美元$ v_2 [k]不是简单地添加到测量中，而是依赖于状态x。在本例中，雷达测量方程是使用Simulink函数块实现的。金宝app

open_system (multirateekf示例/Simuli金宝appnk函数-雷达测量）;

传感器建模- GPS

GPS以1hz的频率测量物体的东、北位置。因此，GPS传感器的测量方程为:

$$ y_{GPS}[k] = \left[ c \开始{数组}{}& # xA;x_e [k] \ \ & # xA;x_n [k] & # xA;结束\{数组}& # xA;\右]+ & # xA;左\ [& # xA;c \开始{数组}{}& # xA;v_1 [k] \ \ & # xA;v_2 [k] & # xA; \end{array} \right] $$

在这里v_1 [k]美元而且美元$ v_2 [k]是测量噪声项，协方差矩阵为[10^2 0;0 10 ^ 2)。也就是说，测量精确到大约10米，误差是不相关的。测量噪声是可加性的，因为噪声项会影响测量美元y_ {GPS} $线性。

创建这个函数，并将其保存在一个名为gpsMeasurementFcn.m．当测量噪声是加性的，你不能在函数中指定噪声项。您可以在扩展卡尔曼滤波器块中提供此函数名和测量噪声协方差。

类型gpsMeasurementFcn
function y = gpsMeasurementFcn(x) % gpsMeasurementFcn状态估计GPS测量函数% %假设状态x为:% [EastPosition;NorthPosition;EastVelocity;上面的%#codegen标签是需要的，你想使用MATLAB Coder为你的过滤器y = x([1 2]) %生成C或c++代码;%位置状态测量结束

滤波器结构

配置扩展卡尔曼滤波块来执行估计。您可以指定状态转换和测量函数名称、初始状态和状态误差协方差以及过程和测量噪声特征。

在系统模型选项卡，指定以下参数:

状态转换

  1. 指定状态转换函数，stateTransitionFcn,在函数．既然你有这个函数的雅可比矩阵，选择雅可比矩阵，并指定雅可比函数，stateTransitionJacobianFcn．

  2. 选择非相加在过程噪声下拉列表，因为您明确声明了进程噪声如何影响函数中的状态。

  3. 指定过程噪声协方差为[0.2 0;0 0.2]。如在植物建模在本例的部分中，过程噪声术语定义了速度在每个方向上的随机游走。对角线项大致捕捉了在状态转换函数的一个采样时间内速度可以改变多少。非对角线项设为零，这是一个天真的假设，即北方向和东方向的速度变化不相关。

初始化

  1. 中指定您的最佳初始状态估计初始状态．在这个例子中，指定[100;100;0;0]。

  2. 说明你对你的状态估计猜测的信心最初的协方差．本例中为10。软件将此值解释为可能在以内的真实状态值下午\ \ sqrt{10} $美元你的初步估计。您可以通过设置为每个状态指定一个单独的值最初的协方差作为一个向量。通过将不确定性指定为矩阵，可以指定不确定性中的交叉相关性。

由于有两个传感器，请单击添加测量指定第二个测量函数。

测量1

  1. 指定测量函数的名称，radarMeasurementFcn,在函数．

  2. 选择非相加在测量噪声下拉列表，因为您明确说明了过程噪声如何影响函数中的度量。

  3. 指定测量噪声协方差为[0.05^2 0;0 0.05^2]传感器建模-雷达部分。

测量2

  1. 指定测量函数的名称，gpsMeasurementFcn,在函数．

  2. 该传感器模型有附加噪声。因此，指定GPS测量噪声为添加剂在测量噪声下拉列表。

  3. 指定测量噪声协方差为[100 0;0 100]。

在多重速率的Tab，由于两个传感器工作在不同的采样率下，执行以下配置:

  1. 选择启用多速率操作．

  2. 指定状态转换采样时间。状态转换采样时间必须最小，所有测量采样时间必须是状态转换采样时间的整数倍。指定状态转换采样时间为0.05，采样时间测量最快。虽然在本例中不需要，但状态转换的采样时间可能比所有测量都要小。这意味着在没有任何测量的情况下会有一些采样时间。对于这些采样时间，过滤器使用状态转换函数生成状态预测。

  3. 指定测量1采样时间(雷达)为0.05秒和测量2(GPS)为1秒。

模拟及结果

通过模拟一个场景来测试扩展卡尔曼滤波器的性能，该场景中物体以以下动作以正方形模式移动:

  • t = 0时，物体从美元x_e (0) = 100 \;\textnormal{[m]}， x_n(0)=100 \;\textnormal{[m]}$

  • 它在$ {x} \点_n”= 50 \;\ textnormal {[m / s]} $直到t = 20秒。

  • 它在$ {x} \点_n”= 40 \;\ textnormal {(m / s)} $t = 20和t = 45秒之间。

  • 它在$ {x} \点_n”= -25 \ \ textnormal {[m / s]} $t = 45和t = 85秒之间。

  • 它在\ \点{x} _e = -10美元;\ textnormal {[m / s]} $在t = 85到185秒之间。

生成该运动对应的真实状态值:

Ts = 0.05;% [s]真实状态的抽样率[t, xTrue] = generateTrueStates(Ts);生成0-185秒内的位置和速度剖面

模拟模型。例如，看看东方方向的实际速度和估计速度:

sim卡(“multirateEKFExample”）;open_system (' multirateekf示例/范围-东速度'）;

图中显示了东方向的真实速度，以及它的扩展卡尔曼滤波估计。过滤器成功地跟踪了速度的变化。滤波器的多速率特性在t = 20到30秒的时间范围内最为明显。该滤波器每秒进行较大的校正(GPS采样率)，而雷达测量的校正每0.05秒可见一次。

下一个步骤

  1. 验证状态估计:无气味和扩展卡尔曼滤波器性能的验证通常使用广泛的蒙特卡罗模拟完成。有关更多信息，请参见在Simulink中验证在线状态估计金宝app．

  2. 生成代码:Unscented和Extended Kalman Filter模块支持使用Simulink Coder™软件生成C金宝app和c++代码。金宝app向这些模块提供的函数必须符合MATLAB Coder™软件(如果您正在使用MATLAB函数对系统建模)和Simulink Coder软件(如果您正在使用Simulink函数模块对系统建模)的限制。金宝app

总结

这个例子展示了如何使用系统识别工具箱中的扩展卡尔曼滤波器块。你通过两个不同的传感器以不同的采样率工作来估计物体的位置和速度。

close_system (“multirateEKFExample”, 0);

另请参阅

||

相关的话题