介绍

控制系统工具箱™为非线性状态估计提供三个命令：

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

此示例首先使用undentedkalmanfilter命令演示此工作流程。然后它表明使用粒子滤光器。

工厂建模和离散化

无味卡尔曼滤波器（UKF）算法要求描述了从一个时间步骤到下一个状态的演进的功能。这通常被称为状态转换函数。unscentedKalmanFilter支金宝app持以下两种功能形式：

这里F（..）是状态转换功能，X是的状态下，w是过程噪声。u是可选的，并且表示附加输入到F，例如系统输入或参数。你可以指定为零或多个函数参数。附加噪声意味着状态和过程噪声是线性相关的。如果关系是非线性的，请使用第二种形式。当你创造的时候undentedkalmanfilter对象，您指定F（..）以及过程噪声是否是附加的或非添加剂。

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

表示这方程式 $$\underset{}{\overset{\dot{}}{X}}=F_C（X）$$， 在哪里 $$X=[X_1;X_2]$$．过程噪声的硬件还没有出现在系统模型。你可以假定它是简单相加。

unscentedKalmanFilter需要离散时间状态转换函数，但工厂模型是连续时间。您可以使用离散时间逼近连续时间模型。欧拉离散化是一个普通的逼近方法。假设您的采样时间为 $${\mathrm{T.}}_{\mathrm{S.}}$$，并表示您拥有的连续时间动态 $$\underset{}{\overset{\dot{}}{X}}=F_C（X）$$．欧拉离散化近似导数算子为 $$\underset{}{\overset{\dot{}}{X}}\approx \frac{X[\mathrm{K.}+1]-X[\mathrm{K.}]}{{\mathrm{T.}}_{\mathrm{S.}}}$$．由此产生的离散时间 - 转换功能是：

这种近似的准确性取决于采样时间 $${\mathrm{T.}}_{\mathrm{S.}}$$．较小 $${\mathrm{T.}}_{\mathrm{S.}}$$值提供更好的近似值。或者，您可以使用不同的离散化方法。例如，考虑到固定采样时间，更高阶runge-Kutta的方法以牺牲更高的计算成本为代价提供更高的准确性 $${\mathrm{T.}}_{\mathrm{S.}}$$．

创建此状态转换功能并将其保存在名为VDPSTATEFCN.M的文件中。使用采样时间 $${\mathrm{T.}}_{\mathrm{S.}}=0.．0.5.\mathrm{S.}$$．你提供了这个功能undentedkalmanfilter在物体建设期间。

AddPath（FullFile（Matlabroot，“例子”那'控制'那'主要的'）））%添加示例数据类型vdpstatefcn.

功能x = vdpstatefcn（x）％vdpstatefcn用于Mu = 1.％采样时间的Van der POL杂物的离散时间近似为0.05s。用于离散时间非线性状态％估算器的%%示例状态转换函数。%% xk1 = vdpstatefcn（xk）％输入：％xk  - 状态x [k]％％输出：％xk1  - 传播状态x [k + 1] %%另见ExtendedKalmanFilter，UnstentedkalmanFilter％2016 MathWorks，Inc。％＃codegen％标签％＃codegen如果要使用％MATLAB编码器生成代码，则必须包括。连续时间动态x'= f（x）的％euler积分具有采样时间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)];结尾

传感器建模

undentedkalmanfilter还需要一个函数，描述模型状态如何与传感器测量有关。undentedkalmanfilter金宝app支持以下两个功能表格：

H（..）是测量功能，V.是测量噪音。你是可选的，代表额外的输入H，例如系统输入或参数。U可以指定为零个或多个函数参数。您可以添加附加的系统输入你学期。这些输入可以是比在状态转换函数的输入不同。

对于这个例子，假设你有第一状态的测量值 $$X_1$$在某些百分比中错误：

这落入了非加性测量噪声的类别，因为测量噪声不会简单地添加到状态的函数中。你想估计两者 $$X_1$$和 $$X_2$$从嘈杂的测量。

创建此状态转换功能并将其保存在名为的文件中vdpMeasurementNonAdditiveNoiseFcn.m．你提供了这个功能undentedkalmanfilter在物体建设期间。

类型vdpmeasurementnonadditivenoisefcn.

功能YK = vdpMeasurementNonAdditiveNoiseFcn（XK，VK）％vdpMeasurementNonAdditiveNoiseFcn实施例的测量与非附加测量噪声离散％的时间非线性状态估计功能。％％YK = vdpNonAdditiveMeasurementFcn（XK，VK）％％输入：％XK  -  X [k]的，在时间状态ķ％VK  -  v [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

无创的卡尔曼滤波器建设

通过向状态转换和测量函数提供功能处理，然后初始状态猜测来构造过滤器。状态转换模型具有附加噪声。这是过滤器中的默认设置，因此您无需指定它。测量模型具有非加性噪声，您必须通过设置来指定hasadditivemeasurementnoise.对象的属性错误的．这必须在对象构造期间完成。如果您的应用程序在状态转移函数中具有非加性过程噪声，请指定HasaddittyProcessnoise.财产错误的．

%您在时间k时的初始状态猜测，利用到时间k-1的测量:xhat[k|k-1]initialSteguess = [2; 0];％xhat [k | k-1]％构造过滤器Ukf = UnscentedkalmanFilter（......@vdpStateFcn，......％状态转换功能@vdpMeasurementNonAdditiveNoiseFcn，......％测量功能initialStateGuess,......'hasadditivemeasurementnoise'，错误的）;

提供您对测量噪声协方差的了解

r = 0.2;测量噪声的v的方差％[k]的ukf。MeasurementNoise = R;

这processnoise.属性存储流程噪声协方差。它被设定为模型不准确性以及未知扰动对植物的影响。我们在这个例子中拥有真实模型，但离散化引入了错误。此示例不包括用于简单性的任何干扰。将其设置为对角矩阵，在第一状态上具有较少噪声的对角线矩阵，并且在第二状态上更多地反映第二状态通过建模错误影响的知识。

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

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

在您的应用程序中，来自硬件的实时测量数据在到达时由过滤器进行处理。这个操作在本例中演示了，首先生成一组测量数据，然后一步一步地将数据输入过滤器。

使用滤波器采样时间0.05 [S]模拟van der POL振荡器5秒钟以生成系统的真正状态。

t = 0.05;％[S]过滤器采样时间TimeVector = 0：T：5;[〜，xtrue] = ode45（@ VDP1，TimeVector，[2; 0]）;

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

rng (1);％修正了可重复的结果的随机数发生器ytrue = xtrue（：，1）;YMEAS = YTRUE。*（1 + SQRT（R）* RANDN（尺寸（ytrue）））;%根号(R):噪声标准差

为稍后要分析的数据预分配空间

nsteps = numel（ymeas）;％步骤数量Xcorritedukf =零（nsteps，2）;％校正状态估计pcorrited = zeros（nsteps，2,2）;％的校正状态估计误差协方差e = 0 (Nsteps, 1);％残差（或创新）

执行在线估计国家X使用正确的和预测命令。一次向滤波器提供生成的数据一次。

为了k = 1: Nsteps％让k表示当前时间。％％残差（或创新）：测量的输出 - 预测输出e（k）= ymeas（k） -  vdpmeasurementfcn（Ukf.state）;此时，％UKF.State是x [k | k-1]将k时刻的测量值纳入到状态估计中％使用“正确”命令。此更新状态和StateCovariance%属性过滤器包含x[k|k]和P[k|k]。这些值％也产生作为“正确”的命令的输出。[Xcorratedukf（k，:)，pcorrited（k，：，:)] =正确（UKF，YMEAS（K））;％在下一次步骤中预测状态K + 1。此更新状态和% StateCovariance属性的过滤器包含x[k+1|k]和％P [K + 1 | k]的。这些将通过在下一时间步骤中的过滤器中使用。预测（UKF）;结尾

Unscented Kalman筛选结果和验证

随着时间的推移绘制真实和估计的状态。还绘制了第一州的测量值。

图();次要情节(2,1,1);情节（timeVector，xTrue（：，1），timeVector，xCorrectedUKF（：，1），timeVector，yMeas（:)）;传奇（“真正的”那'UKF估计'那'衡量'）ylim（[ -  2.6 2.6]）;ylabel（'x_1'）;子图（2,1,2）;绘图（TimeVector，Xtrue（:,2），TimeVector，XcorratedukF（:,2））;ylim（[ -  3 1.5]）;Xlabel（'时间[S]'）;ylabel（'x_2'）;

顶部绘图显示了第一个状态的真实，估计和测量值。过滤器利用系统模型和噪声协方差信息，以在测量结果上产生改进的估计。底部图显示了第二个状态。过滤器是成功的，生产良好的估计。

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

图();绘图（TimeVector，E）;Xlabel（'时间[S]'）;ylabel（“剩余(或创新)”）;

剩余部分应包括:

残差的平均值是：

平均（E）

ans = -0.0012.

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

[xe，xelags] = xcorr（e，'coeff'）;％'coeff'：通过零滞后的值正常化％只绘制非负滞后idx = xelags> = 0;图();绘图（XELAGS（IDX），XE（IDX））;Xlabel（'滞后'）;ylabel（'归一化相关'）;标题('残留的自相关（创新）'）;

相关性是很小的所有滞后，除了0的平均相关性接近零，相关不显示任何显著非随机变化。这些特点增加过滤性能的信心。

在现实中，真正的状态是永远可用。然而，在进行模拟时，你有机会获得真正的状态，并可以查看估计和真实状态之间的误差。这些错误必须满足类似的标准的残余：

首先，看看错误和 $$1\sigma$$从过滤误差协方差估计误差范围。

estates = xtrue-xcorritedukf;图();次要情节(2,1,1);绘图（TimeVector，Estates（：1），......第一个州的％错误TimeVector，SQRT（PCorrited（：，1,1）），'r'那......％1-sigma上限TimeVector，-sqrt（pcorrited（：，1,1）），'r'）;% 1-sigma下界Xlabel（'时间[S]'）;ylabel（'状态1'错误）;标题('国家估计错误'）;子图（2,1,2）;绘图（TimeVector，Estates（:,2），......％误差的第二状态TimeVector，SQRT（Pcorrited（：，2,2）），'r'那......％1-sigma上限TimeVector，-SQRT（PCorrited（:,2,2）），'r'）;% 1-sigma下界Xlabel（'时间[S]'）;ylabel（'状态2'错误）;传奇（'国家估计'那“1-sigma不确定性约束”那......'地点'那'最好的事物'）;

由于传感器模型，在T = 2.15秒内接近状态1的误差（MeasurementFCN.）。它的形式是 $$X_1[\mathrm{K.}]*（1+\mathrm{V.}[\mathrm{K.}]）$$．在T = 2.15秒时，真实和估计的状态接近零，这意味着绝对术语中的测量误差也在零附近。这反映在过滤器的状态估计误差协方差中。

计算点数的百分比超出了1-sigma不确定性绑定。

distanceFromBound1 = abs(地产(:1))-√(PCorrected (:, 1, 1));percentageExceeded1 = nnz(distanceFromBound1>0) / numel(eStates(:，1));distanceFromBound2 = abs(地产(:,2))-√(PCorrected (:, 2, 2));percentageExceeded2 = nnz(distanceFromBound2>0) / numel(eStates(:，2));[percentageExceeded1 percentageExceeded2]

ans =.1×20.1386 0.

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

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

意思（庄园）

ans =.1×2-0.0103 0.0201

[xestates1，xestateslags1] = Xcorr（估值（：，1），'coeff'）;％'coeff'：通过零滞后的值正常化[xestates2，xestateslags2] = Xcorr（估值（:,2），'coeff'）;％'coeff'％只绘制非负滞后idx = xeStatesLags1 > = 0;图();次要情节(2,1,1);情节(xeStatesLags1 (idx) xeStates1 (idx));Xlabel（'滞后'）;ylabel（'对于州1'）;标题('状态估计误差的标准化自相关'）;子图（2,1,2）;绘图（Xestateslags2（idx），xestates2（idx））;Xlabel（'滞后'）;ylabel（'对于州2'）;

误差的平均值相对于州的值很小。状态估计误差的自相关显示小滞后值的非随机变化很小，但是这些比率小于归一化峰值1.结合估计状态准确的事实，残留物的这种行为可以被视为令人满意结果。

粒子过滤器结构

无限和扩展的卡尔曼滤波器旨在跟踪状态估计的后部分布的平均值和协方差不同的近似方法。如果系统中的非线性严重，则这些方法可能是不够的。此外，对于一些应用，只需跟踪状态估计的后部分布的平均值和协方差可能是不够的。粒子过滤器可以通过追逐许多状态假设（粒子）的演变，以牺牲更高的计算成本来解决这些问题。计算成本和估计精度随粒子的数量增加。

这粒子滤片命令实现离散时间粒子滤波算法。这一节将指导您构建一个粒子滤片对于此示例前面使用的相同的范德波振荡器，并突出显示与Unscented Kalman滤波器的相似性和差异。

你提供的状态转换功能粒子滤片必须执行两个任务。一个，对系统的任何分布采样对过程噪声。二，计算所有粒子（状态假设）到下一步骤的时间传播，包括在步骤1中计算的过程噪声的效果。

类型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

有你提供给状态转移函数之间的差异undentedkalmanfilter和粒子滤片．您用于Unspented Kalman滤波器的状态转换功能刚刚将一个状态假设的传播描述为下次步骤，而不是一组假设。此外，在过程中定义了过程噪声分布processnoise.财产的undentedkalmanfilter，只是通过其协方差。粒子滤片可以考虑可能需要定义更多统计属性的任意发行版。在您提供的状态转换功能中完全定义该任意分布及其参数粒子滤片．

你提供的度量可能性函数粒子滤片还必须执行两个任务。一个，从粒子计算测量假设。二，计算每个粒子的可能性来自传感器测量和在步骤1中计算的假设。

类型VDPEXAMPLEPFMEASURENTLIKELIOOWFCN.

function likelihood = vdpexampleepfmeasurementlikelihood dfcn (particles,measurement) % vdpexampleepfmeasurementlikelihood function % %测量是第一状态。% % likelihood = vdpparticlefiltermeasurementlikelihood dfcn (particles, measurement) % % Inputs: % particles - numberofstates -by numberofparticles矩阵，包含%粒子%输出:%可能性-一个向量与NumberOfParticles元素n %的元素是第n个粒子% %的可能性也看到extendedKalmanFilter unscentedKalmanFilter % 2017年版权MathWorks公司% # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。%确认传感器测量值为1;%测量的预期数量验证属性(测量，{'double'}， {'vector'， 'numel'， numberOfMeasurements}，…“vdpExamplePFMeasurementLikelihoodFcn”、“测量”);%测量是第一状态。从predictedMeasurement = particles(1，:)中得到所有的测量假设;假设预测误差与实际测量误差之比服从均值为零的高斯分布，方差0.2 mu = 0;% mean sigma = 0.2 * eye(numberOfMeasurements); % variance % Use multivariate Gaussian probability density function, calculate % likelihood of each particle numParticles = size(particles,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 = plaretfilter（@ vdpparticlefilterstatefcn，@ vdpexamplepfmeasurementlikelihoodfcn）;初始化（PF，1000，[2; 0]，0.01 *眼睛（2））;

可选地，选择状态估计方法。这是由最终于司法方法财产粒子滤片，可以取值'意思'（默认）或'maxweight'．什么时候最终于司法方法是'意思'，物体从中提取颗粒的加权平均值粒子和重量属性作为国家估计。'maxweight'对应于选择具有最大重量值的粒子（状态假设）重量作为国家估计。或者，您可以访问粒子和重量和所述对象的特性通过您选择的任意方法提取您状态估计。

pf.stateestimationMethod.

ANS = '平均'

粒子滤片让您指定各种重采样选项通过它ResamplingPolicy和重新制动方法特性。此示例使用过滤器中的默认设置。看看粒子滤片关于重新取样的进一步细节的文件。

pf.resamplingmethod.

ANS ='多项式'

pf.ResamplingPolicy

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

启动估计循环。这表示逐步到达的测量值。

％ 估计xcorrectpf = zeros（尺寸（xtrue））;为了k = 1：尺寸（xtrue，1）％用途测量Y [k]的，以校正时间k处的颗粒xcorrectpf（k，:) =正确（pf，ymeas（k））;% Filter更新和存储粒子[k|k]，权重[k|k]％结果是X [k | k]：利用时，估计状态k，利用％测量到时间k。这种估计是所有粒子的平均值％，因为endyestimationMethod是“平均”。％现在，预测粒子在下一个时间步骤。这些在％，明年正确的命令预测（PF）;％过滤器更新和存储粒子[k + 1 | k]结尾

绘制粒子滤波器的状态估计:

图();次要情节(2,1,1);情节(timeVector xTrue (: 1), timeVector, xCorrectedPF (: 1), timeVector, yMeas (:));传奇（“真正的”那“Particlte滤波器估计”那'衡量'）ylim（[ -  2.6 2.6]）;ylabel（'x_1'）;子图（2,1,2）;绘图（TimeVector，Xtrue（:,2），TimeVector，XcorrectPF（:,2））;ylim（[ -  3 1.5]）;Xlabel（'时间[S]'）;ylabel（'x_2'）;

顶部图显示了真实值，粒子滤波器估计和第一状态的测量值。过滤器利用系统模型和噪声信息来在测量结果上产生改进的估计。底部图显示了第二个状态。过滤器成功地产生了良好的估计。

粒子滤波器性能的验证涉及对残差进行统计测试，类似于在该示例中以前在此示例中执行的那些，以便未选择的卡尔曼滤波结果。

总结

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

rmpath（完整文件（matlabroot，“例子”那'控制'那'主要的'）））％删除示例数据