介绍

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

使用这些命令的典型流程如下:

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

植物建模与离散化

无迹卡尔曼滤波(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在对象构造。

addpath（完整文件（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$$从噪声测量。

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

类型VDP测量非加性ENOISEFCN

示例测量函数用于含非加性测量噪声的离散%时间非线性状态估计器。% % yk = vdpNonAdditiveMeasurementFcn(xk,vk) % %输入:% xk - x[k]，时刻k % vk - v[k]，时刻k测量噪声% %输出:% yk - y [k],测量时k % %带乘性噪声测量是第一个国家% %也看到extendedKalmanFilter unscentedKalmanFilter % 2016年版权MathWorks公司% # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。yk = xk (1) * (1 + vk);结束

无迹卡尔曼滤波构造

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

%您在时间k时的初始状态猜测，利用到时间k-1的测量: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;

的过程噪声属性存储过程噪声协方差。它被设置为考虑模型的不准确性和未知干扰对电厂的影响。在这个例子中我们有真实的模型，但是离散化会带来错误。为了简单起见，本例中没有包含任何干扰。将其设置为一个对角线矩阵，在第一个状态上噪声更少，在第二个状态上噪声更多，以反映第二状态受建模错误影响更大的知识。

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

使用predict和correct命令进行估计

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

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

T = 0.05;% [s]过滤采样时间timeVector = 0:师:5;[~, xTrue] =数值(@vdp1、timeVector [2; 0]);

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

RNG（1）;%修复随机数生成器的可重复结果yTrue = xTrue (: 1);yMeas = yTrue .* (1+sqrt(R)*randn(size(yTrue))));％SQRT（R）：噪声的标准偏差

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

Nsteps =元素个数(yMeas);%时间步长xCorrectedUKF = 0 (Nsteps, 2);%修正的状态估计PCorrected = 0 (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时刻的测量值纳入到状态估计中%使用“正确”命令。这将更新状态和状态协方差%属性过滤器包含x[k|k]和P[k|k]。这些值%也会作为“correct”命令的输出产生。[xCorrectedUKF (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, xCorrectedUKF (: 1), timeVector, yMeas (:));传奇(“真正的”，“UKF估计”，“测量”)ylim（[-2.6 2.6]）；伊拉贝尔(“x_1”）;次要情节(2,1,2);情节(timeVector xTrue (:, 2), timeVector, xCorrectedUKF (:, 2));ylim (1.5 [3]);包含(“时间[s]”）;ylabel (“x_2”）;

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

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

图();情节(timeVector e);包含(“时间[s]”）;ylabel (“剩余(或创新)”）;

残差应该：

残差的均值为:

意思是(e)

ans = -0.0012

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

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

除了0。平均相关系数接近于零，且相关系数没有明显的非随机变化。这些特性增加了对过滤器性能的信心。

在现实中，真正的状态是永远不可用的。然而，在执行模拟时，您可以访问真实状态，并可以查看估计状态和真实状态之间的误差。这些误差必须满足与残差相似的标准:

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

地产= xTrue-xCorrectedUKF;图();次要情节(2,1,1);情节(timeVector地产(:1),.．.第一个状态的错误timeVector,√PCorrected: 1 1)),“r”，.．.% 1-sigma上限时间向量，-sqrt（PCorrected（：，1,1）），“r”）;% 1-sigma下界包含(“时间[s]”）;ylabel ('状态1的错误'）;标题（状态估计误差的）;次要情节(2,1,2);情节(timeVector地产(:,2),.．.第二个状态的错误timeVector,√PCorrected: 2 2)),“r”，.．.% 1-sigma上限timeVector -√(PCorrected (: 2 2)),“r”）;% 1-sigma下界包含(“时间[s]”）;ylabel ('状态2的错误'）;传奇(状态估计的，“1-sigma不确定性约束”，.．.“位置”，“最佳”）;

由于传感器型号的原因，状态1的误差范围在t=2.15秒时接近0(测量)．它有这样的形式 $$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×20.1386 0

第一个状态估计误差超过 $$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));包含(“滞后”）;ylabel (“针对国家1”）;标题（“状态估计误差的归一化自相关”)子地块（2,1,2）；地块（xeStatesLags2（idx），xeStates2（idx））；xlabel(“滞后”）;ylabel (“为国家2”）;

误差的平均值相对于状态值来说很小。状态估计误差的自相关在滞后值较小的情况下表现出很小的非随机变化，但这些变化远小于归一化峰值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

你提供的状态转移函数之间是有区别的unscentedKalmanFilter和颗粒过滤器。用于unscented Kalman filter的状态转移函数只是描述了一个状态假设到下一个时间步的传播，而不是一组假设。此外，过程噪声分布在过程噪声财产的财产unscentedKalmanFilter，仅通过其协方差。颗粒过滤器可以考虑需要定义更多统计属性的任意分布。这个任意分布及其参数完全定义在您提供的状态转移函数中颗粒过滤器．

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

类型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 = particleFilter (@vdpParticleFilterStateFcn @vdpExamplePFMeasurementLikelihoodFcn);初始化(pf, 1000， [2;0]， 0.01*eye(2));

（可选）选择状态估计方法。这是由StateEstimationMethod性质颗粒过滤器，可以取值“中庸”(默认)或“最大重量”．当StateEstimationMethod是“中庸”，该对象从中提取粒子的加权平均值粒子和权重属性作为状态估计。“最大重量”对应于在中选择权重值最大的粒子(状态假设)权重作为国家的估计。或者，您可以访问粒子和权重属性，并通过您选择的任意方法提取您的状态估计。

状态估计法

ans = '的意思是'

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

pf.ResamplingMethod

ans = '多项'

pf.resamplingpolicy.

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

启动估算循环。这表示随着时间的推移逐步到达的测量值。

%估计xCorrectedPF = 0(大小(xTrue));对于k = 1:尺寸(xTrue, 1)%使用测量y[k]来修正时间k的粒子xCorrectedPF (k) =正确(pf, yMeas (k));% Filter更新和存储粒子[k|k]，权重[k|k]%结果是x[k | k]：估计时间k的状态，利用这个估计值是所有粒子的平均值%，因为statestimationmethod是mean。％％现在，在下次步骤中预测粒子。这些是利用的% next正确命令预测(pf);%过滤器更新并存储粒子[k+1 | k]结束

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

图();次要情节(2,1,1);绘图（TimeVector，Xtrue（：1），TimeVector，XcorrectPF（：，1），TimeVector，Ymeas（:)）;传奇(“真正的”，“Particlte滤波器估计”，“测量”)ylim（[-2.6 2.6]）；伊拉贝尔(“x_1”); 子批次（2,1,2）；绘图（时间向量，xTrue（：，2），时间向量，xCorrectedPF（：，2））；ylim（[-31.5]）；xlabel(“时间[s]”）;ylabel (“x_2”）;

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

粒子滤波器性能的验证涉及对残差进行统计测试，类似于本示例前面针对无迹卡尔曼滤波器结果所执行的测试。

概括

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

rmpath (fullfile (matlabroot,'例子'，“控制”，“主要的”))%删除示例数据