非线性系统的估计国多，多速率传感器

这个示例使用:

打开脚本

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

介绍

该工具箱具有非线性状态估计3个Simulin万博1manbetxk模块：

扩展卡尔曼滤波:实现一阶离散时间扩展卡尔曼滤波算法。
无迹卡尔曼滤波:实现离散时间无迹卡尔曼滤波算法。
粒子滤波:实现离散时间粒子滤波算法。

这些块支持使用在不同的采样万博1manbetx速率操作的多个传感器的状态估计。用于使用这些块的典型工作流程如下：

使用MATLAB或Simulink函数对植物和传感器的行为建模。万博1manbetx
配置块的参数。
模拟滤波器和分析结果，以获得在过滤性能的信心。
在硬件上部署过滤器。您可以使用Simulink Coder™软件为这些过滤器生成代码。万博1manbetx

此示例使用扩展的Kalman过滤器块演示此工作流的前两个步骤。最后两个步骤将在本文中简要讨论下一个步骤部分。本例的目标是使用雷达和GPS传感器提供的噪声测量来估计对象的状态。对象的状态是它的位置和速度，在Simulink模型中表示为xTrue。万博1manbetx

如果你有兴趣在颗粒过滤器块，请参见例如“使用参数和状态估计在Simulink粒子滤波模块”。万博1manbetx

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

植物建模

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

添加剂过程噪声：
非相加过程噪声:

其中f(..)为状态转移函数，x为状态，w为过程噪声。u是可选的，表示f的其他输入，例如实例系统输入或参数。加性噪声是指下一个状态 $ X [K + 1] $ 和过程噪声 $ W [k]的$ 线性相关。如果关系是非线性的，使用非加形式。

函数f(…)可以是符合MATLAB Coder™限制的MATLAB函数，也可以是Simulink函数块。万博1manbetx在创建f(…)之后，指定函数名，并在扩展的卡尔曼滤波块中指定进程噪声是可添加的还是不可添加的。

在本例中，您将跟踪一个物体在二维平面上的东北方向的位置和速度。估计数量如下:

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

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

$A = \左[ \begin{array}{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函数块实现状态转换函数。万博1manbetx要做到这一点,

添加一个万博1manbetxSimulink的功能阻止到您的模型万博1manbetx模型/用户定义函数图书馆
单击Simulink函数块上显示的名称。万博1manbetx编辑函数名，并根据需要添加或删除输入和输出参数。在本例中，状态转换函数的名称为stateTransitionFcn。它有一个输出参数(xNext)和两个输入参数(x, w)。

虽然在本例中并不需要，但是您可以在Simulink函数中使用来自其余Simulink模型的任何信号。万博1manbetx为此，添加轮廓尺寸块的万博1manbetx模型/来源图书馆。注意，它们与ArgIn和ArgOut被通过您的函数的签名集（xNext = stateTransitionFcn（X，W））的块。
在Simuli万博1manbetxnk函数块中，使用Simulink块构造函数。
方法中设置输入和输出参数x、w和xNext的维数信号的属性选项卡的ArgIn和ArgOut块。数据类型和端口的尺寸必须与您在提供的信息一致扩展卡尔曼滤波器块。

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

open_system (“multirateEKFExample 万博1manbetx/ Simulink的功能 - 状态转换雅可比”);

传感器建模-雷达

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

添加剂测量噪声:
非相加测量噪声:

其中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%。测量噪声是非加性的，因为和不是简单地添加到测量值中，而是依赖于状态x。在本例中，雷达测量方程是使用Simulink函数块实现的。万博1manbetx

open_system ('multirateEKFExample 万博1manbetx/ Simulink的功能 - 雷达测量');

传感器建模- GPS

一个GPS在1赫兹测量对象的东部和北部位置。因此，对于GPS传感器的测量方程为：

$y_{GPS}[k] = \左[ 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为:%[东方网;NorthPosition;EastVelocity;上面需要的是你想用MATLAB编码器生成C或c++代码的过滤器y = x([1 2]);位置状态测量结束

滤波器结构

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

在系统模型块对话框的选项卡，指定以下参数:

状态转换

指定状态转换函数，stateTransitionFcn,在函数。既然你有这个函数的雅可比，选择雅可比矩阵，然后指定雅可比函数，stateTransitionJacobianFcn。
选择非相加在过程噪声下拉列表中，因为你明确规定了如何处理噪音影响各国的功能。
指定过程噪声协方差为[0.2 0;0 0.2]。如在植物建模在这个例子中，过程噪声项定义了速度在每个方向上的随机游走。对角项近似地反映了状态转移函数在一个采样时间内的速度变化量。非对角项设置为零，这是一个天真的假设，速度变化在北部和东部方向是不相关的。

初始化

指定您的最佳初始状态估计值初始状态。在本例中，指定[100;100;0;0]。
指定你的信心在你的状态估计猜测最初的协方差。在本例中，指定10。软件将此值解释为可能包含的真实状态值 $下午\ \ sqrt{10} $美元$ 你最初的估计。您可以通过设置为每个状态指定一个单独的值最初的协方差作为一个向量。您可以通过将其指定为矩阵来指定这种不确定性中的相互关系。

因为有两个传感器，所以单击添加测量指定第二个度量函数。

测量1

指定测量函数的名称，radarMeasurementFcn,在函数。
选择非相加在测量噪声下拉列表中，因为你明确说明如何处理噪音影响你的函数测量。
指定测量噪声协方差为[0.05^2 0;0。05^2]传感器建模-雷达部分。

测量2

指定测量函数的名称，gpsMeasurementFcn,在函数。
该传感器模型具有加性噪声。因此，指定GPS测量噪音添加剂在测量噪声下拉列表。
指定测量噪声协方差为[100 0;0 100]。

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

选择使多重速率的操作。
指定的状态转变采样时间。状态转换采样时间必须是最小的，并且所有测量样品时间必须的状态转换的采样时间的整数倍。指定状态转换采样时间为0.05，采样时间的测量最快。虽然在本例中不是必需的，但是与所有测量相比，状态转换的采样时间可能更短。这意味着在没有测量的情况下会有一些采样时间。对于这些样本时间，过滤器使用状态转换函数生成状态预测。
指定测量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和t = 185秒之间。

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

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

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

SIM（'multirateEKFExample');open_system (多速率kfexample /Scope - East Velocity);

图中显示的真实速度在东部方向，它的扩展卡尔曼滤波估计。该过滤器成功地跟踪速度的变化。过滤器的多速率性质是在时间范围t = 20〜30秒最为明显。该过滤使得大校正每秒（GPS采样率），而由于雷达测量的校正是可见每0.05秒。

下一个步骤

验证状态估计:无迹和扩展卡尔曼滤波性能的验证通常使用大量的蒙特卡罗模拟来完成。有关更多信息，请参见验证在线状态估计在Simulink万博1manbetx。
生成代码:Unscented和扩展的Kalman过滤器块支持使用Simulink Coder™软件生成C和c++代码。万博1manbetx万博1manbetx您提供给这些块的函数必须符合MATLAB Coder™软件(如果您使用MATLAB函数对系统建模)和Simulink Coder软件(如果您使用Simulink函数块对系统建模)的限制。万博1manbetx