介绍

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

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

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

植物建模与离散化

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

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

该示例中的系统是带MU = 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（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万博1manbetx支持以下两种功能形式:

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

无迹卡尔曼滤波构造

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

％您的初始状态猜测在时间k，利用测量值k-1：xhat [k | k-1]initialSteguess = [2; 0];% xhat (k | k - 1)%构建过滤器ukf = unscentedKalmanFilter (．..@vdpstatefcn，．..状态转移函数@vdpMeasurementNonAdditiveNoiseFcn，．..%测量功能initnstateguess，．..'hasadditivemeasurementnoise'、假);

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

R = 0.2;测量噪声V [K]的％方差ukf.measurementnoise = r;

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

Ukf.processnoise = diag（[0.02 0.1]）;

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

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

模拟范德堡尔振荡器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 =零（nsteps，1）;剩余(或创新)%

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

为了k = 1：nsteps％让K表示当前时间。％残差(或创新):测量产出-预测产出e(k) = yMeas(k) - vdpMeasurementFcn(ukf.State);% ukf。在这一点状态是x[k|k-1]％将时间k加入状态估计值％使用“正确”命令。此更新状态和StateCovariance过滤器的％性质含有X [k | k]和P [k | k]。这些价值观％也作为“正确”命令的输出产生。[xCorrectedUKF (k,:), PCorrected (k,:,:)] =正确(ukf, yMeas (k));％在下一次步骤中预测状态K + 1。此更新状态和滤波器的％stemovariance属性包含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, xCorrectedUKF (:, 2));ylim (1.5 [3]);包含(“时间[s]”）;ylabel（“x_2”）;

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

通常使用广泛的Monte Carlo模拟进行无需和扩展卡尔曼滤波器性能的验证。这些模拟应测试过程和测量噪声的变化，在各种条件下运行的工厂，初始状态和州协方差猜测。用于验证状态估计的关键兴趣信号是残差（或创新）。在此示例中，您对单个模拟执行残余分析。绘制残差。

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

残差应该：

残差的均值为:

意思（e）

ans = -0.0012

这相对于残差的大小很小。可以使用信号处理工具箱中的XCorr功能计算残差的自相关。

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

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

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

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

地产= xTrue-xCorrectedUKF;图();次要情节(2,1,1);情节(timeVector地产(:1),．..第一个状态的错误timeVector,√PCorrected: 1 1)),“r”，．..% 1-sigma上限TimeVector，-sqrt（pcorrited（：，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不确定性绑定'，．..“位置”，“最佳”）;

由于传感器模型，在T = 2.15秒内接近状态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×20.1386 0

第一个状态估计误差超过了 $$1\mathrm{\sigma .}$$不确定性约束约14％的时间步骤。不到30％的错误超过1-sigma不确定性绑定意味着良好的估计。这两个州都满足该标准。第二州的0％百分比意味着过滤器是保守的：最有可能组合过程和测量噪声太高。可能通过调整这些参数来获得更好的性能。

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

意思（庄园）

ans =.1×2-0.0103 - 0.0201

[xeStates1, xeStatesLags1] = xcorr(地产(:1),多项式系数的）;% 'coeff':按零延迟时的值进行正常化[xestates2，xestateslags2] = Xcorr（估值（:,2），多项式系数的）;％'coeff'％只绘制非负滞后idx = xestateslags1> = 0;图();次要情节(2,1,1);绘图（Xestateslags1（IDX），Xestates1（IDX））;包含(“滞后”）;ylabel（“状态1”）;标题（'状态估计误差的标准化自相关'）;次要情节(2,1,2);绘图（Xestateslags2（idx），xestates2（idx））;包含(“滞后”）;ylabel（'对于州2'）;

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

粒子滤波结构

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

的粒子滤片控制系统工具箱中的命令实现了一个离散时间粒子滤波器算法。这节走进通过构造一个粒子滤片，并强调了它与无迹卡尔曼滤波器的异同。

你提供的状态转换功能粒子滤片必须执行两个任务。第一，从适合您的系统的任何分布中采样过程噪声。第二，计算所有粒子到下一步的时间传播(状态假设)，包括第一步中计算的过程噪声的影响。

类型vdpparticlefilterstatefcn.

功能粒子= Vdpparticlefilterstatefcn（粒子）％Vdpparticlefilterstatefcn实施例的粒子％过滤功能对于MU = 1.％采样时间的普拉％滤波器％离散时间近似的尺寸为0.05秒。％％预测颗粒= VdpparticlefilterstateFCN（粒子）％％输入：％粒子 - 当前时间颗粒。具有尺寸的矩阵％[numbumofstates numberofparticles]矩阵%%输出：％预测颗粒 - 下次预测粒子的步骤%%另见粒子福特％2017 The MathWorks，Inc.％＃Codegen％Tage％＃Codegen如果您必须包括标签％＃codegen希望使用％MATLAB编码器生成代码。[numberofstates，numberofparticle] =尺寸（粒子）;％时间 - 传播连续时间动态x'= f（x）的每个粒子%% euler积分，采样时间dt dt = 0.05;kk = 1：numberofparticle颗粒（：，kk）=粒子（：，kk）+ vdpstatefcn连续（粒子（：，kk））* dt;终端％在每个状态变量流程= 0.025 *眼睛上加入高斯噪声粒子=粒子+ ProcessNoise * Randn（尺寸（粒子））;计算mu = 1时的van der Pol ode dxdt = [x(2); (1-x(1)^2)*x(2)-x(1)]; end

您提供的状态转换功能之间存在差异unscentedKalmanFilter和粒子滤片．您用于Unspented Kalman滤波器的状态转换功能刚刚将一个状态假设的传播描述为下次步骤，而不是一组假设。此外，在过程中定义了过程噪声分布processnoise.财产的财产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财产粒子滤片，这可以占据价值'意思'(默认)或'maxweight'．当StateEstimationMethod是'意思'，该对象从中提取粒子的加权平均值粒子和权重属性作为状态估计。'maxweight'对应于在中选择权重值最大的粒子(状态假设)权重作为国家的估计。或者，您可以访问粒子和权重对象的属性通过您选择的任意方法提取您的状态估算。

pf.stateestimationMethod.

ans ='卑鄙'

粒子滤片允许您通过其指定各种重新采样选项重新采样策划和ResamplingMethod特性。此示例使用过滤器中的默认设置。看看粒子滤片关于重新采样的进一步详细信息的文档。

pf.ResamplingMethod

ans = '多项'

pf.resamplingpolicy.

ANS = ParticleresamplingPolicy具有属性：Triggermethod：'比率'SamplingInterval：1 MineFfectiveParticleratio：0.5000

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

％ 估计xCorrectedPF = 0(大小(xTrue));为了k = 1:尺寸(xTrue, 1)％使用测量y [k]校正时间k的粒子xCorrectedPF (k) =正确(pf, yMeas (k));％过滤器更新并存储粒子[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]）;ylabel（“x_1”）;次要情节(2,1,2);绘图（TimeVector，Xtrue（:,2），TimeVector，XcorrectPF（:,2））;ylim (1.5 [3]);包含(“时间[s]”）;ylabel（“x_2”）;

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

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

概括

该示例已经示出了构造和使用Unscented Kalman滤波器的步骤和用于非线性系统的状态估计的粒子滤波器。您从嘈杂测量估计van der POL振荡器的状态，并验证了估计性能。

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