介绍

Control System Toolbox™提供了三个用于非线性状态估计的命令:

使用这些命令的典型工作流如下所示：

本例首先使用unscentedKalmanFilter命令以演示此工作流。然后它演示了particleFilter的使用。

植物建模与离散化

Unscented Kalman滤波器（UKF）算法需要一个函数，该函数从一个时间步骤到下一个时刻来描述状态的演变。这通常称为状态转换函数。UnscentedkalmanFilter支金宝app持以下两个功能表单：

在这里f（..）是状态转移函数，x是国家，w是过程噪音。U是可选的，代表额外的输入f，例如系统输入或参数。u可以指定为零个或多个函数参数。加性噪声是指状态噪声和过程噪声是线性相关的。如果关系是非线性的，则使用第二种形式。当您创建unscentedKalmanFilter对象，您可以指定f(..)以及过程噪声是加性的还是非加性的。

这个例子中的系统是=1的范德堡尔振子。这个双态系统用以下一组非线性常微分方程(ODE)描述:

将这个方程表示为 $$\underset{}{\overset{\dot{}}{x}}＝f_c（x）$$， 在哪里 $$x＝［x_1；x_2］$$．过程噪声W不会出现在系统模型中。您可以假设它是简单的添加剂。

UnscentedKalmanFilter需要离散时间状态转换功能，但工厂模型是连续的。您可以使用连续时间模型的离散时间近似。欧拉离散化是一种常见的近似方法。假设您的采样时间是 $$T_{\mathrm{年代}}$$，表示连续时间动力学，如 $$\underset{}{\overset{\dot{}}{x}}＝f_c（x）$$．欧拉离散化近似于衍生操作员 $$\underset{}{\overset{\dot{}}{x}}\approx \frac{x［k+1］-x［k］}{T_{\mathrm{年代}}}$$．得到的离散时间状态转移函数为:

这种近似的准确性取决于采样时间 $$T_{\mathrm{年代}}$$．较小 $$T_{\mathrm{年代}}$$数值提供了更好的近似值。或者，您可以使用不同的离散化方法。例如，在给定固定采样时间的情况下，高阶龙格-库塔方法系列以更高的计算成本为代价提供了更高的精度 $$T_{\mathrm{年代}}$$．

创建这个状态转换函数并将其保存在名为vdpStateFcn.m的文件中。使用样本时间 $$T_{\mathrm{年代}}＝0．05\mathrm{年代}$$．将此函数提供给unscentedKalmanFilter在对象构造。

目录(fullfile (matlabroot,'例子'，'控制'，“主要”））%添加示例数据类型vdpStateFcn

函数x = vdpStateFcn(x) % vdpStateFcn对mu = 1的范德堡尔ode的离散时间近似。%采样时间为0.05s。用于离散时间非线性状态估计的状态转移函数示例。% % % % xk1 = vdpStateFcn (xk)输入:% xk -州x [k] % %输出:% xk1 -传播国家x [k + 1] % %也看到extendedKalmanFilter unscentedKalmanFilter % 2016年版权MathWorks公司% # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。连续时间动力学的% Euler积分x'=f(x)与采样时间dt dt = 0.05;% [s]采样时间x = x + vdpStateFcnContinuous(x)*dt;计算mu = 1时的van der Pol ode dxdt = [x(2);(1 - x (1) ^ 2) * (2) x - x (1)];结束

传感器建模

unscentedKalmanFilter还需要一个函数来描述模型状态如何与传感器测量相关联。unscentedKalmanFilter金宝app支持以下两种功能形式:

h（..）是测量功能，v是测量噪声。u是可选的，并表示额外的输入h，例如系统输入或参数。您可以指定为零或多个函数参数。您可以在以下内添加其他系统输入u学期。这些输入可以与状态转换函数中的输入不同。

对于此示例，假设您有第一个状态的测量值 $$x_1$$在一定百分比误差范围内：

这属于非加性测量噪声的范畴，因为测量噪声不是简单地添加到状态函数中。两者都要估计 $$x_1$$和 $$x_2$$从噪声测量。

创建状态转换函数并将其保存在名为VDPMEASURENTENONADDTIVENOISEFCN.M.M.．将此函数提供给unscentedKalmanFilter在对象构造。

类型vdpMeasurementNonAdditiveNoiseFcn

功能YK = VDPMEASUREMENTNONADDTIVENOISEFCN（XK，VK）％VDPMEASURENCENONADDITIVENOISEFCN示例测量功能对于具有非加上附加测量噪声的离散％非线性状态估计的测量功能。%% YK = VDPNONADDitiveMeasUrementFCN（XK，VK）％Inputs：％XK  -  X [k]，在时间K％VK-V [K]，测量噪声k％k％％输出：％yk  -  y [k]，measurements at time k % % The measurement is the first state with multiplicative noise % % See also extendedKalmanFilter, unscentedKalmanFilter % Copyright 2016 The MathWorks, Inc. %#codegen % The tag %#codegen must be included if you wish to generate code with % MATLAB Coder. yk = xk(1)*(1+vk); end

无迹卡尔曼滤波构造

通过为状态转换和度量函数提供函数句柄，然后提供初始状态猜测来构造筛选器。状态转换模型具有附加噪波。这是筛选器中的默认设置，因此无需指定。度量模型具有非附加噪波，必须通过setti指定把HasAdditiveMeasurementNoise对象的属性假．这必须在对象构造期间完成。如果您的应用程序在状态转移函数中具有非加性过程噪声，请指定HasAdditiveProcessNoise财产作为假．

%利用时间k-1之前的测量值，在时间k进行初始状态猜测：xhat[k | k-1]initialStateGuess = (2; 0);% xhat (k | k - 1)%构建过滤器ukf = unscentedKalmanFilter (...@vdpstatefcn，...状态转移函数@vdpmeasurementnonadditivenoisefcn，...%测量功能initialStateGuess,...“HasAdditiveMeasurementNoise”，假）；

提供测量噪声协方差的知识

R=0.2；测量噪声V [K]的％方差ukf。MeasurementNoise = R;

的ProcessNoise属性存储过程噪声协方差。它被设置为考虑模型不精确性和未知干扰对设备的影响。在本例中，我们有真实的模型，但离散化会引入错误。为简单起见，本例不包括任何干扰。将其设置为第一状态噪声较少的对角矩阵d更多关于第二种状态的信息，以反映第二种状态受建模错误影响更大的知识。

ukf。ProcessNoise = diag([0.02 0.1]);

使用预测和正确的命令进行估计

在您的应用程序中，从您到达时，从您的硬件到达的测量数据由筛选器进行处理。在该示例中首先生成一组测量数据，然后一次将其馈送到滤波器，在该操作中进行了说明该操作。

模拟范德波尔振荡器5秒，滤波器采样时间为0.05[s]，以生成系统的真实状态。

T = 0.05;% [s]过滤采样时间时间向量=0:T:5；[~，xTrue]=ode45（@vdp1，时间向量[2；0]）；

生成测量假设一个传感器测量第一个状态，每个测量误差的标准偏差为45%。

RNG（1）;％修复随机数发生器以进行可重复的结果yTrue=xTrue（：，1）；yMeas=yTrue.*（1+sqrt（R）*randn（大小（yTrue））；％SQRT（R）：噪声的标准偏差

预先分配的空间，以便稍后分析

Nsteps =元素个数(yMeas);%时间步数xCorrectedUKF = 0 (Nsteps, 2);修正状态估计值PCorrected=零（Nsteps，2,2）；％纠正态估计错误考核e = 0 (Nsteps, 1);剩余(或创新)%

执行状态的在线估计x使用正确的和预测命令。一次一个时间步骤向过滤器提供生成的数据。

为k = 1: Nsteps％让K表示当前时间。％%残差（或创新）：测量输出-预测输出e(k) = yMeas(k) - vdpMeasurementFcn(ukf.State);% ukf。在这一点状态是x[k|k-1]将k时刻的测量值纳入到状态估计中%使用“correct”命令。这将更新State和StateCovariance%属性过滤器包含x[k|k]和P[k|k]。这些值％也作为“正确”命令的输出产生。[xcorrectedkf（k，：），PCorrected（k，：，：）]=正确（ukf，yMeas（k））；%预测下一个时刻k+1的状态。这更新了国家和% StateCovariance属性的过滤器包含x[k+1|k]和％p [k + 1 | k]。这些将由滤波器在下次步骤中使用。预测(ukf);结束

无迹卡尔曼滤波结果与验证

绘制随时间变化的真实和估计状态。同时绘制第一个状态的测量值。

图();次要情节(2,1,1);绘图（TimeVector，Xtrue（：，1），TimeVector，XcorritedUKF（：，1），TimeVector，Ymeas（:)）;传奇（“真正的”，“UKF估算”，“测量”) ylim ([-2.6 - 2.6]);ylabel (“x_1”）;次要情节(2,1,2);情节(timeVector xTrue (:, 2), timeVector, xCorrectedUKF (:, 2));ylim (1.5 [3]);包含(“时间[s]”)；伊莱贝尔(“x_2”）;

上面的图显示了第一个状态的真实值、估计值和测量值。该滤波器利用系统模型和噪声协方差信息对测量值产生改进的估计。下面的图显示了第二种状态。该过滤器成功地产生了一个良好的估计。

通常使用广泛的蒙特卡罗模拟来验证无气味和扩展卡尔曼滤波器的性能。这些模拟应该测试过程和测量噪声实现的变化，在各种条件下运行的工厂，初始状态和状态协方差猜测。用于验证状态估计的关键信号是残差(或创新)。在本例中，您将对单个模拟执行残留分析。绘制残差。

图（）；绘图（时间向量，e）；xlabel(“时间[s]”)；伊莱贝尔(“剩余(或创新)”）;

残差应该：

残差的均值为:

意思（e）

ans=-0.0012

这相对于残差的大小来说很小。残差的自相关可以用信号处理工具箱中的xcorr函数来计算。

[xe, xeLags] = xcorr (e,多项式系数的）;% 'coeff':按零延迟时的值进行正常化%只绘制非负滞后idx = xeLags > = 0;图();情节(xeLags (idx), xe (idx));包含(“滞后”)；伊莱贝尔(“归一化相关性”）;标题（“残差的自相关(创新)”）;

除了0之外，相关性对于0的滞后，相关性是小的。平均相关性接近于零，并且相关性不会显示任何显着的非随机变化。这些特性增加了对滤波器性能的置信度。

实际上，真正的国家永远不会有。但是，在执行模拟时，您可以访问真实状态，可以查看估计和真正的状态之间的错误。这些错误必须满足残差的类似标准：

首先，看看错误和 $$1\mathrm{\sigma .}$$从过滤器错误协方差估计的不确定性界限。

地产= xTrue-xCorrectedUKF;图();次要情节(2,1,1);情节(timeVector地产(:1),...%第一个状态的错误timeVector,√PCorrected: 1 1)),“r”，...%1-西格玛上界timeVector -√(PCorrected (:, 1, 1)),“r”）;% 1-sigma下界包含(“时间[s]”)；伊莱贝尔('状态1出错'）;标题（状态估计误差的)子地块（2,1,2）；地块（时间向量，不动产（：，2），...第二个状态的％错误时间向量，sqrt（PCorrected（：，2,2）），“r”，...%1-西格玛上界时间向量，-sqrt（PCorrected（：，2,2）），“r”）;% 1-sigma下界包含(“时间[s]”)；伊莱贝尔('状态2出错'）;传奇（“状态估计”，“1-西格玛不确定度界”，...“位置”，“最佳”）;

由于传感器模型(MeasurementFcn）。它具有表单 $$x_1［k］＊（1+v［k］）$$．在t=2.15秒时，真实状态和估计状态接近于零，这意味着测量误差的绝对值也接近于零。这反映在滤波器的状态估计误差协方差中。

计算超过1 σ不确定性界限的百分比。

TextingFrombound1 = ABS（庄园（：，1）） -  SQRT（PCORRETTED（：，1,1））;百分比xceeded1 = nnz（距离框架1> 0）/ numel（庄园（：，1））;TextingFrombound2 = ABS（庄园（：，2）） -  SQRT（PCORRECTED（：，2,2））;百分比xceeded2 = nnz（距离比较2> 0）/ numel（庄园（：，2））;[百分比xceeded1百分比xceeded2]

ans =1×2零点一三八六零

第一个状态估计误差超过 $$1\mathrm{\sigma .}$$不确定性的界限大约是14%的时间步长。超过1 σ不确定性界限的误差小于30%意味着良好的估计。两种状态都满足这个标准。第二种状态的0%表示滤波器是保守的:很可能合并的过程和测量噪声太高。通过调优这些参数，可能可以获得更好的性能。

计算状态估计误差的平均自相关，并绘制自相关图。

意思是(地产)

ans =1×2-0.0103 0.0201

[xeStates1，xeStatesLags1]=xcorr（不动产（：，1），多项式系数的）;% 'coeff':按零延迟时的值进行正常化[xeStates2, xeStatesLags2] = xcorr(地产(:,2),多项式系数的）;%的多项式系数%只绘制非负滞后idx = xeStatesLags1 > = 0;图();次要情节(2,1,1);情节(xeStatesLags1 (idx) xeStates1 (idx));包含(“滞后”)；伊莱贝尔(“状态1”）;标题（状态估计误差的归一化自相关）;次要情节(2,1,2);情节(xeStatesLags2 (idx) xeStates2 (idx));包含(“滞后”)；伊莱贝尔(《国家2》）;

误差的平均值相对于状态值来说很小。状态估计误差的自相关在滞后值较小的情况下表现出很小的非随机变化，但这些变化远小于归一化峰值1。结合估计状态是准确的这一事实，残差的这种行为可以被认为是令人满意的结果。

粒子滤波器结构

无迹扩展卡尔曼滤波器的目的是通过不同的逼近方法跟踪状态估计的后验分布的均值和协方差。如果系统中的非线性很严重，这些方法可能是不够的。此外，对于某些应用，仅仅跟踪状态估计的后验分布的均值和协方差可能是不够的。粒子滤波器可以通过跟踪许多状态假设(粒子)随时间的演化来解决这些问题，但代价是较高的计算成本。计算代价和估计精度随粒子数的增加而增加。

的particleFilter命令实现离散时间粒子滤波算法。这一节将指导您构建一个particleFilter对于本示例前面使用的同一个范德波尔振荡器，突出显示了与无迹卡尔曼滤波器的异同。

你提供的状态转移函数particleFilter必须执行两项任务。第一，从适合您的系统的任何分布中采样过程噪声。第二，计算所有粒子（状态假设）到下一步的时间传播，包括在第一步中计算的过程噪声的影响。

类型vdpparticlefilterstatefcn.

用于粒子% filter % %的状态转移函数示例%采样时间为0.05s。% % predictedParticles = vdpParticleFilterStateFcn(particles) % %输入:% particles -当前时间的粒子。矩阵维度% [NumberOfStates NumberOfParticles]矩阵输出% %:% predictedParticles——预测粒子为下一个时间步% %参见particleFilter % 2017年版权MathWorks, Inc . % # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。[numberOfStates, numberOfParticles] = size(粒子);连续时间动力学的Euler积分x'=f(x)与采样时间dt dt = 0.05;% [s] Sample time for kk=1:numberOfParticles particles(:，kk) = particles(:，kk) + vdpStateFcnContinuous(particles(:，kk))*dt;processNoise = 0.025*eye(numberOfStates);粒子=粒子+ processNoise * randn(size(粒子));计算mu = 1时的van der Pol ode dxdt = [x(2); (1-x(1)^2)*x(2)-x(1)]; end

您提供的状态转换功能之间存在差异unscentedKalmanFilter和particleFilter．你用来做无迹卡尔曼滤波的状态转移函数描述了一个状态假设到下一个时间步骤的传播，而不是一组假设。此外，过程噪声分布定义在ProcessNoise财产的财产unscentedKalmanFilter也就是协方差。particleFilter可以考虑任意的分布，可能需要定义更多的统计特性。在提供的状态转换函数中，可以完全定义任意分布及其参数。particleFilter．

您提供的测量似然功能particleFilter还必须执行两个任务。一，计算粒子的测量假设。第二，从传感器测量和第一步中计算的假设中计算每个粒子的可能性。

类型vdpExamplePFMeasurementLikelihoodFcn

功能似然= vdpexamplepfmeAsurementlikelihoodfcn（粒子，测量）％vdpexamplepfmeasurementlikelihoodfcn示例测量似然函数%%测量是第一个状态。％％似然= Vdpparticlefiltermeasurementlikelihoothfcn（颗粒，测量）％输入：％粒子 - 逐个Numberof partix，其保持％粒子％％的产出：％似然性 - 具有Numberofparly元素的载体，其N-Th％元素是其N-Th％元素的载体第n粒粒子%%另见ExtendedKalmanFilter，UnstentedKalmanFilter％2017 MathWorks，Inc.％＃Codegen％如果要生成％Matlab编码器，则必须包括标签％＃Codegen。％验证传感器测量NumberofmeAreents = 1;％预期的测量数有validateattributes（测量，{'double'}，{vector'，'numel'，numberofmeasurements}，...'vdpexamplepfmeasurementlikelihoodfcn'，'measurement'）;％测量是第一个状态。从粒子预测=粒子中获取所有测量假设=粒子（1，:);％假设预测和实际测量之间的误差的比率遵循零平均值的高斯分布，方差0.2μm= 0;％平均sigma = 0.2 *眼睛（numberofmeasurese）;％方差％使用多变量高斯概率密度函数，计算每个粒子的似然性=尺寸（粒子，2）; likelihood = zeros(numParticles,1); C = det(2*pi*sigma) ^ (-0.5); for kk=1:numParticles errorRatio = (predictedMeasurement(kk)-measurement)/predictedMeasurement(kk); v = errorRatio-mu; likelihood(kk) = C * exp(-0.5 * (v' / sigma * v) ); end end

现在构建过滤器，并初始化它的1000个粒子左右的均值[2;0]具有0.01的协方差。协方差很小因为你对你的猜测很有信心[2;0]。

pf=粒子过滤器（@vdpParticleFilterStateFcn，@vdpExamplePFMeasurementLikelihoodFcn）；初始化（pf，1000，[2；0]，0.01*eye（2））；

可以选择状态估计方法。这是由状态估计法的属性particleFilter，它可以接受该值“的意思是”(默认)或“maxweight”．当状态估计法是“的意思是”，对象将从中提取粒子的加权平均值粒子和权重属性作为状态估计。“maxweight”对应于选择中具有最大权重值的粒子（状态假设）权重根据州政府的估计。或者，您可以访问粒子和权重对象的属性通过您选择的任意方法提取您的状态估算。

pf.StateEstimationMethod

ans ='卑鄙'

particleFilter允许您通过其指定各种重新采样选项重新采样策划和ResamplingMethod属性。此示例使用过滤器中的默认设置。看到particleFilter关于重新取样的进一步细节的文件。

重采样法

ans=‘多项式’

pf.resamplingpolicy.

ans = particlersamamplingpolicy with properties: TriggerMethod: 'ratio' SamplingInterval: 1 mineffecveparticleratio: 0.5000

启动估计循环。这表示随着时间一步一步地到达的测量值。

%的估计xCorrectedPF=零（大小（xTrue））；为k = 1:尺寸(xTrue, 1)％使用测量y [k]校正时间k的粒子xCorrectedPF（k，：）=正确（pf，yMeas（k））；% Filter更新和存储粒子[k|k]，权重[k|k]结果是:在时间k时的状态估计，利用%时间k之前的测量值。此估计值是所有粒子的平均值%因为国家评估方法是“中庸的”。％％现在，在下次步骤中预测粒子。这些是利用的％next正确的命令预测(pf);% Filter更新和存储粒子[k+1|k]结束

从粒子过滤器绘制状态估计值：

图();次要情节(2,1,1);绘图（TimeVector，Xtrue（：1），TimeVector，XcorrectPF（：，1），TimeVector，Ymeas（:)）;传奇（“真正的”，'particlte过滤估计'，“测量”) ylim ([-2.6 - 2.6]);ylabel (“x_1”）;次要情节(2,1,2);情节(timeVector xTrue (:, 2), timeVector, xCorrectedPF (:, 2));ylim (1.5 [3]);包含(“时间[s]”)；伊莱贝尔(“x_2”）;

上面的图显示了第一状态的真值、粒子滤波估计和测量值。该滤波器利用系统模型和噪声信息对测量值产生改进的估计。下面的图显示了第二种状态。该过滤器成功地产生了一个良好的估计。

粒子滤波器性能的验证包括对残差进行统计测试，类似于本例中早些时候对无迹卡尔曼滤波器结果进行的测试。

概括

这个例子展示了构造和使用无迹卡尔曼滤波器和粒子滤波器对非线性系统状态估计的步骤。你从噪声测量中估计了范德堡尔振荡器的状态，并验证了估计性能。

rmpath（fullfile（matlabroot，'例子'，'控制'，“主要”））%删除示例数据