主要内容

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

这个示例使用:

打开脚本

这个例子展示了如何执行非线性状态估计模型™系统与多个传感器操作在不同的采样率。金宝app扩展卡尔曼滤波器在系统辨识工具箱™是用来估计物体的位置和速度使用全球定位系统(GPS)和雷达测量。

介绍

工具箱有三个模型块非线性状态估计:金宝app

  • 扩展的卡尔曼滤波器:实现了一阶离散时间扩展卡尔曼滤波算法。

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

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

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

  1. 你们工厂模型和传感器行为使用MATLAB或仿真软件的功能。金宝app

  2. 配置块的参数。

  3. 模拟和分析结果进行筛选,获得信心在滤波器的性能。

  4. 你的硬件上部署过滤器。您可以为这些过滤器生成代码使用仿真软件编码器™软件。金宝app

下面的例子使用了扩展卡尔曼滤波块来演示这个工作流的前两个步骤。最后两个步骤简要的讨论下一个步骤部分。这个例子的目的是评估对象的状态使用雷达提供的噪声测量和GPS传感器。对象的状态是它的位置和速度,在仿真软件中被指示为xTrue模型。金宝app

如果你是感兴趣的粒子滤波块,请参阅示例”参数和状态估计模型使用粒子滤波块”。金宝app

目录(fullfile (matlabroot,“例子”,“识别”,“主要”))%添加示例数据open_system (“multirateEKFExample”);

植物建模

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

  • 添加剂过程噪声:$ x [k + 1] = f (x [k], u [k]) + w美元[k]

  • 非相加过程噪声:$ x [k + 1] = f (x [k], w [k], u [k])美元

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

函数f(…)可以是一个MATLAB函数符合MATLAB编码器™的限制,或者一个仿真软件功能块。金宝app在您创建f(…),你指定函数名和过程噪声是否添加剂或非相加的扩展卡尔曼滤波器。

在本例中,您正在跟踪对象的北部和东部位置和速度在一个二维平面。估计的数量是:

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

在这里k美元是离散指数。状态转移方程非相加的形式使用$ {x} \帽子[k + 1] = \帽子{x} [k] + Gw [k]美元,在那里$ \帽子{x} $状态向量,w美元噪音是一个过程。过滤器假设w美元是一个零均值、独立随机变量方差已知的E (ww ^ T)美元。A和G矩阵:

左$ $ = \ [& # xA;\开始{数组}{c c c c} & # xA;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美元样品时间。第三行和G模型的东速度随机漫步:$ {v} \帽子_e (k + 1) = {v} \帽子_e [k] + w_1 [k]美元。在现实中,位置是一个连续时间变量和速度随时间的积分$ \压裂{d} {dt} {x} _e = \ \帽子帽子{v} _e美元。第一行和G代表一个离散近似的运动学关系:美元(\帽子{x} _e [k + 1] - \帽子{x} _e [k]) / T_s =(\帽子{v} _e [k + 1] + \帽子{v} _e [k]) / 2美元。的第二和第四行和G表示相同的北速度和位置之间的关系。这个状态转换模型是线性的,但雷达测量模型是非线性的。这种非线性需要使用非线性状态估计等扩展卡尔曼滤波器。

在这个例子中,您使用一个仿真软件实现状态转换函数功能块。金宝app要做到这一点,

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

  • 点击名称显示在仿真软件功能块。金宝app编辑函数名,添加或删除输入和输出参数,作为必要的。在这个例子中状态转换函数的名称stateTransitionFcn。它有一个输出参数(xNext)和两个输入参数(x, w)。

  • 虽然不需要在这个例子中,您可以使用任何信号的仿真软件模型的仿真软件的功能。金宝app为此,添加轮廓尺寸块的金宝app模型/来源图书馆。请注意,这些都是不同的ArgIn和ArgOut块将通过你的函数的签名(xNext = stateTransitionFcn (x, w))。

  • 仿真软件的功能金宝app块,构造函数利用仿真软件模块。

  • 设置输入和输出参数的尺寸x, w, xNext信号的属性选项卡的ArgIn和ArgOut块。数据类型和端口尺寸必须符合您提供的信息扩展卡尔曼滤波器块。

分析状态转换函数的雅可比矩阵也在这个示例实现。指定的雅可比矩阵是可选的。然而,这减少了计算负担,而且在大多数情况下增加了状态估计精度。实现雅可比矩阵函数作为模型函数,因为状态转换函数是一个仿真软件的功能。金宝app

open_system (“雅可比矩阵multirateEKFExa金宝appmple /仿真软件功能-状态转换”);

传感器建模——雷达

扩展卡尔曼滤波块也需要一个测量函数,描述了美国相关测量。支持以下两种函数形式:金宝app

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

  • 非相加测量噪声:$ y [k] = h (x [k], [k], u [k])美元

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

在这个例子中雷达位于原点措施对象的范围和角度在20 Hz。假设两个测量有大约5%的噪音。这可以通过以下测量方程建模:

$ $ y_{雷达}[k]左= \ [& # xA;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。在这个例子中,雷达测量方程使用仿真软件实现的功能块。金宝app

open_system (“multirateEKFExample 金宝app/仿真软件功能-雷达测量”);

传感器建模——全球定位系统

GPS测量对象的东部和北部的位置在1赫兹。因此,GPS传感器的测量方程是:

$ $ y_ {GPS} [k]左= \ [& # xA;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;NorthVelocity] % % % # # codegen codegen标签上面需要你想使用MATLAB编码器%生成C或c++代码过滤y = x ([1 - 2]);%的位置状态测量结束

滤波器结构

配置扩展卡尔曼滤波块执行评估。你指定状态转换和测量函数名、初始状态和状态误差协方差,和过程和测量噪声特性。

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

状态转换

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

  2. 选择非相加在过程噪声下拉列表,因为你明确噪声过程如何影响美国在你的函数。

  3. 指定过程噪声协方差(0.2 0;0 0.2]。的解释植物建模节的这个例子中,过程噪声条件定义速度的随机漫步在每个方向。对角线项大约捕获多少速度可以改变在一个样本的状态转换函数。非对角的条件都设置为0,这是一个天真的假设速度变化北部和东部方向是互不相关的。

初始化

  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)} $在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 (“东方multirateEKFExample /范围-速度”);

情节展示了真正的东方向的速度,和它的扩展卡尔曼滤波估计。过滤器成功跟踪速度的变化。滤波器是最明显的多重速率的性质在时间范围t = 20到30秒。过滤器使大修正每一秒(GPS采样率),而修正由于雷达测量可见每0.05秒。

下一个步骤

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

  2. 生成代码:无味和扩展卡尔曼滤波模块支持C和c++代码生成使用仿真软件编码器™软件。金宝app金宝app你提供这些功能块的限制必须符合MATLAB编码器™软件(如果你使用MATLAB函数模型系统)和仿真软件编码器软件(如果您正在使用仿真软件功能块模型系统)。金宝app

总结

这个例子展示了如何使用扩展卡尔曼滤波器在系统辨识工具箱。你估计从两个不同的对象的位置和速度传感器操作在不同的采样率。

close_system (“multirateEKFExample”,0);rmpath (fullfile (matlabroot,“例子”,“识别”,“主要”))%删除示例数据

另请参阅

||

相关的话题

系统辨识工具箱文档

金宝app

试着用MATLAB仿真软金宝app件,和其他产品下载188bet金宝搏

得到审判现在