主要内容

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

这个示例使用:

开放脚本

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

介绍

工具箱有三个Simulink块用于非线性状态万博1manbetx估计：

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

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

  • 粒子滤波:实现离散时间粒子滤波算法。

这些块支持使用以不同采样率万博1manbetx运行的多个传感器进行状态估计。使用这些块的典型工作流如下：

  1. 使用MATLAB或Simulink函数建模您的植物和传感器行为。万博1manbetx

  2. 配置块的参数。

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

  4. 在硬件上部署过滤器。您可以使用Simulink Coder™软件为这些过滤器生成代码。万博1manbetx

这个例子使用扩展卡尔曼滤波块来演示这个工作流的前两个步骤。最后两个步骤将在下一个步骤节。本例中的目标是使用雷达和GPS传感器提供的噪声测量值来估计对象的状态。对象的状态是其位置和速度，在Simulink模型中表示为xTrue。万博1manbetx

如果您对粒子滤波块感兴趣，请参阅示例“Simulink中使用粒子滤波块的参数和状态估计”。万博1manbetx

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

植物建模

扩展卡尔曼滤波(EKF)算法需要一个状态转移函数来描述状态从一个时间步骤到下一个时间步骤的演化。该块支持以下两种功能万博1manbetx形式:

  • 附加过程噪声：$x[k+1]=f（x[k]，u[k]）+w[k]$

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

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

函数f（…）可以是符合MATLAB编码器限制的MATLAB函数™, 或Simulink功能块。创建f（…）后，可以在扩展卡尔曼滤波器块中指定函数名以及过程噪声是相加的还是非相加的。万博1manbetx

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

$$ \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{East position estimate} \;[m] \\&# a;\textnormal{方位估计}\; [m] \\ \textnormal{East velocity estimate} \; [m/s] \\ \textnormal{North velocity estimate} \; [m/s] \\ \end{array} $$

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

0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0和0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0#xA；\end{array} ；\right] ；\；\；\；\；\；G=\left[ ；\begin{array}{c} ；T#us/2&；0\ ；0 ；T#s/2\\ ；1&；0\\ 38；1 end{array} $$

哪里$T\s$为采样时间。A和G的第三行模型东面速度为随机游走:$\hat{v}e[k+1]=\hat{v}e[k]+w_1[k]$.实际上，位置是一个连续的时间变量，是速度随时间的积分$\frac{d}{dt}\hat{x}\e=\hat{v}\e$．A和G的第一行表示这个运动学关系的离散近似:$（\hat{x}e[k+1]-\hat{x}e[k]）/T_s=（\hat{v}e[k+1]+\hat{v}e[k]）/2$。A和G的第二行和第四行表示北速度和位置之间的相同关系。该状态转换模型是线性的，但雷达测量模型是非线性的。这种非线性要求使用非线性状态估计器，如扩展卡尔曼滤波器。

在本例中，您使用Simulink函数块实现状态转换函数。万博1manbetx要做到这一点,

  • 添加一个万博1manbetxSimulink函数块到你的模型万博1manbetxSimulink/用户定义函数图书馆

  • 单击显示在Simulink函数块上的名称。万博1manbetx编辑函数名，并根据需要添加或删除输入和输出参数。在本例中，状态转移函数的名称为状态转换．它有一个输出参数(xNext)和两个输入参数(x, w)。

  • 虽然在本例中不是必需的，但您可以在Simulink函数中使用来自Simulink模型其余部分的任何信号。万博1manbetx要做到这一点，添加轮廓尺寸块的万博1manbetx模型/来源图书馆。注意，这些是不同的精氨酸和辩论通过函数签名设置的块（xNext=stateTransitionFcn（x，w））。

  • 在Simuli万博1manbetxnk功能块中，使用Simulink块构造函数。

  • 属性中为输入和输出参数x、w和xNext设置维度信号的属性选项卡的精氨酸和辩论块。数据类型和端口尺寸必须与中提供的信息一致扩展卡尔曼滤波器块

在这个例子中，还实现了状态转移函数的解析雅可比矩阵。可选指定雅可比矩阵。然而，这减少了计算负担，并且在大多数情况下提高了状态估计的准确性。将雅可比函数实现为Simulink函数，因为状态转移函数是Simulink万博1manbetx函数。

open_system (“MultiratekFeExample/万博1manbetxSimulink函数-状态转换雅可比矩阵”）;

传感器建模-雷达

扩展卡尔曼滤波块还需要一个测量函数来描述状态是如何与测量相关的。支持以下两种函数形式:万博1manbetx

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

  • 非相加测量噪声:$y[k]=h（x[k]，v[k]，u[k]）$

这里h（..）是测量函数，v是测量噪声。u是可选的，表示h的附加输入，例如系统输入或参数。这些输入可能不同于状态转换函数中的输入。

在这个例子中，位于原点的雷达以20赫兹的频率测量目标的距离和角度。假设两个测量值的噪声都在5%左右。这可以用以下测量方程来建模:

$$ y_{radar}[k] = \left[ c \开始{数组}{}& # xA;大概{x_n \ [k] ^ 2 + x_e [k] ^ 2} \;(1 + v_1 [k]) \ \ & # xA;量化(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函数块实现的。万博1manbetx

open_system (“MultiratekFeExample/万博1manbetxSimulink功能-雷达测量”）;

传感器建模- GPS

GPS以1 Hz的频率测量物体的东部和北部位置。因此，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
函数y = gpsMeasurementFcn(x) % gpsMeasurementFcn GPS测量函数用于状态估计% %假设状态x为:% [EastPosition;NorthPosition;EastVelocity;上面的%#codegen标签是需要的，你想使用MATLAB Coder为你的过滤器y = x([1 2]) %生成C或c++代码;%位置状态测量结束

过滤结构

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

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

状态转换

  1. 指定状态转移函数，状态转换,在作用.由于您具有此函数的雅可比矩阵，请选择雅可比矩阵，并指定雅可比函数，状态转换JacobianFCN．

  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]。

在多重速率的选项卡，由于两个传感器以不同的采样率工作，请执行以下配置：

  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)} $在20秒到45秒之间。

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

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

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

t = 0.05;% [s]真实状态的采样率[t，xTrue]=生成状态（Ts）；%在0-185秒内生成位置和速度剖面

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

模拟(“Multiratekfexample”）;open_system ('multirateEKFExample/Scope - East Velocity'）;

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

下一个步骤

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

  2. 生成代码：UnScand和Kalman滤波器块支持Simulink Coder的C和C++代码生成™ 软件。为这些块提供的函数万博1manbetx必须符合MATLAB Coder的限制™ 软件（如果使用MATLAB函数对系统建万博1manbetx模）和Simulink编码器软件（如果使用Simulink功能块对系统建模）。

总结

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

close_system (“Multiratekfexample”, 0);

另请参阅

||

相关话题

控制系统工具箱文档

万博1manbetx

学习如何自动调整PID控制器增益

学习如何自动调整PID控制器增益

下载代码示例