介绍

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

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

本例首先使用非中心过滤器命令来演示此工作流。然后它演示particleFilter的使用。

工厂建模和离散化

无迹卡尔曼滤波(UKF)算法需要一个函数来描述从一个时间步骤到下一个时间步骤的状态演化。这通常被称为状态转移函数。unscentedKalmanFilter支金宝app持以下两种函数形式:

在这里F（..）是状态转移函数，x为状态，w为过程噪声。U是可选的，表示附加的输入F，例如系统输入或参数。U可以指定为零个或多个函数参数。加性噪声是指状态噪声和过程噪声是线性相关的。如果关系是非线性的，则使用第二种形式。当您创建非中心过滤器对象，您可以指定F（..）以及过程噪声是加性噪声还是非加性噪声。

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

将该方程表示为 $$\underset{}{\overset{\dot{}}{x}}=F_C(x)$$,在那里 $$x=[x_{1.};x_{2.}]$$．进程噪声w没有出现在系统模型中。为了简单起见，您可以假设它是加法的。

unscentedKalmanFilter需要一个离散时间的状态转移函数，但被试模型是连续时间的。你可以对连续时间模型使用离散时间近似。欧拉离散法是一种常用的逼近方法。假设你的样本时间是 $$T_s$$，并将连续时间动力学表示为 $$\underset{}{\overset{\dot{}}{x}}=F_C(x)$$．欧拉离散化近似导数算子为 $$\underset{}{\overset{\dot{}}{x}}\approx \frac{x[K+1.]-x[K]}{T_s}$$.由此产生的离散时间状态转换函数为：

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

创建此状态转换函数并将其保存在名为vdpStateFcn.m的文件中。使用采样时间 $$T_s=0．05.s$$. 将此函数提供给非中心过滤器在对象构造期间。

addpath（完整文件（matlabroot、，“例子”,“控制”,“主要的”))%添加示例数据类型vdpStateFcn

函数x=vdpStateFcn（x）%vdpStateFcn离散时间近似于mu=1的范德极采样时间为0.05秒%%离散时间非线性状态%估计量的状态转移函数示例%%xk1=vdpStateFcn（xk）%%输入：%xk-状态x[k]%输出：%xk1-传播状态x[k+1]%另请参见extendedKalmanFilter，UncentedkalmanFilter%版权所有2016 MathWorks，Inc.%#codegen%如果您希望使用%MATLAB Coder生成代码，则必须包含标记%#codegen.%连续时间动力学的欧拉积分x'=f（x），采样时间dt=0.05；%[s] 采样时间x=x+vdpstatefc连续（x）*dt；端函数dxdt=vdpStateFcnContinuous（x）%vdpStateFcnContinuous评估mu=1的范德极dxdt=[x（2）；（1-x（1）^2）*x（2）-x（1）]；终止

传感器建模

非中心过滤器还需要一个描述模型状态如何与传感器测量相关的函数。非中心过滤器金宝app支持以下两种功能形式：

H（..）是测量功能，v是测量噪声。U是可选的，表示到的其他输入H，例如系统输入或参数。U可以指定为零个或多个函数参数。您可以添加附加的系统输入U术语。这些输入可以不同于状态转移函数中的输入。

对于这个例子，假设你有第一状态的测量值 $$x_{1.}$$在一定百分比误差范围内：

这属于非加性测量噪声的范畴，因为测量噪声不是简单地添加到状态的函数中。您希望同时估计这两种噪声 $$x_{1.}$$和 $$x_{2.}$$从嘈杂的测量中。

创建此状态转换函数并将其保存在名为vdpMeasurementNonAdditiveNoiseFcn.m. 将此函数提供给非中心过滤器在对象构造期间。

类型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);终止

无迹卡尔曼滤波器构造

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

%您在时间k时的初始状态猜测，利用到时间k-1的测量:xhat[k|k-1]initialStateGuess=[2；0]；%xhat[k | k-1]%构造过滤器ukf=未经中心化的alManiflter(...@vdpStateFcn,...%状态转移函数@vdpMeasurementNonAdditiveNoiseFcn,...%测量功能initialStateGuess,...“HasAdditiveMeasurementNoise”，假）；

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

R=0.2；测量噪声方差% v[k]ukf。MeasurementNoise = R;

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

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

使用predict和correct命令进行估计

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

模拟范德波尔振荡器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））；%根号(R):噪声标准差

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

Nsteps=努美尔（yMeas）；%时间步数xCorrectedUKF=零（Nsteps，2）；%修正的状态估计PCorrected=零（Nsteps，2,2）；修正状态估计误差协方差e = 0 (Nsteps, 1);%剩余（或创新）

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

对于k = 1: Nsteps用k表示当前时间。%%残差（或创新）：测量输出-预测输出e（k）=yMeas（k）-vdpMeasurementFcn（英国国家）；%此时，ukf状态为x[k | k-1]将k时刻的测量值纳入到状态估计中%使用“正确”命令。这将更新状态和状态协方差%属性过滤器包含x[k|k]和P[k|k]。这些值%也会作为“correct”命令的输出产生。[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, xCorrectedUKF (: 1), timeVector, yMeas (:));传奇(“真正的”,“UKF估算”,“测量”)ylim（[-2.6 2.6]）；伊拉贝尔(“x_1”)子图（2,1,2）；绘图（时间向量，xTrue（：，2），时间向量，xCorrectedUKF（：，2））；ylim（[-3 1.5]）；xlabel(‘时间’)；伊莱贝尔(“x_2”）;

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

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

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

剩余部分应包括:

残差的平均值为：

意思是(e)

ans=-0.0012

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

[xe，xeLags]=xcorr（e，“科夫”）;%“coeff”：通过零滞后时的值进行规格化%仅绘制非负滞后idx=xeLags>=0；图（）；绘图（xeLags（idx），xe（idx））；xlabel(“滞后”)；伊莱贝尔(“归一化相关性”）;标题(‘残差自相关（创新）’）;

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

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

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

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

由于传感器型号的原因，状态1的误差范围在t=2.15秒时接近0(测量）.它有这样的形式 $$x_{1.}[K]*(1.+v[K])$$。在t=2.15秒时，真实状态和估计状态接近于零，这意味着绝对测量误差也接近于零。这反映在滤波器的状态估计误差协方差中。

计算超出1西格玛不确定度界限的分数百分比。

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×2零点一三八六零

第一个状态估计误差超过 $$1.\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））；xlabel(“滞后”)；伊莱贝尔(“为国家2”）;

误差的平均值相对于状态值较小，状态估计误差的自相关对小滞后值几乎没有非随机变化，但比归一化峰值1小得多，结合估计状态准确的事实，可以考虑残差的这种行为。结果令人满意。

粒子滤波器结构

Unscented和extended Kalman滤波器旨在通过不同的近似方法跟踪状态估计后验分布的均值和协方差。如果系统中的非线性很严重，这些方法可能是不够的。此外，对于某些应用，仅跟踪状态估计的后验分布的均值和协方差可能是不够的。粒子滤波器可以通过跟踪许多状态假设（粒子）随时间的演化来解决这些问题，但要付出更高的计算成本。计算量和估计精度随粒子数的增加而增加。

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

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

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

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

你提供的度量可能性函数颗粒过滤器还必须执行两项任务。第一，从粒子计算测量假设。第二，根据传感器测量和第一步计算的假设计算每个粒子的可能性。

类型vdpExamplePFMeasurementLikelihoodFcn

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=粒子过滤器（@vdpParticleFilterStateFcn，@vdpExamplePFMeasurementLikelihoodFcn）；初始化（pf，1000，[2；0]，0.01*eye（2））；

（可选）选择状态估计方法。这是由状态估计法性质颗粒过滤器，可以取值“中庸”（默认）或“最大重量”什么时候状态估计法是“中庸”，对象将从中提取粒子的加权平均值粒子和砝码属性作为状态估计。“最大重量”对应于选择中具有最大权重值的粒子（状态假设）砝码根据州政府的估计。或者，您可以访问粒子和砝码属性，并通过您选择的任意方法提取您的状态估计。

状态估计法

ans = '的意思是'

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

重采样法

ans=‘多项式’

pf.ResamplingPolicy

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

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

%估计xCorrectedPF=零（大小（xTrue））；对于k=1：尺寸（X图，1）%使用测量y[k]来修正时间k的粒子xCorrectedPF（k，：）=正确（pf，yMeas（k））；% Filter更新和存储粒子[k|k]，权重[k|k]%结果是x[k | k]：估计时间k的状态，利用%时间k之前的测量值。此估计值是所有粒子的平均值%因为国家评估方法是“中庸的”。%现在，预测粒子在下一个时间步骤。这些在% next正确命令预测（pf）；%过滤器更新并存储粒子[k+1 | k]终止

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

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

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

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

总结

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

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