介绍

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

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

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

工厂建模和离散化

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

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

本例中的系统是mu=1的范德波尔振荡器。该2态系统由以下一组非线性常微分方程（ODE）描述：

表示这方程式 $$\underset{}{\overset{\dot{}}{X}}=F_C（X）$$,在那里 $$X=[X_1;X_2]$$．进程噪声w没有出现在系统模型中。为了简单起见，您可以假设它是加法的。

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.}}$$值提供更好的近似。或者，您可以使用不同的离散化方法。例如，在给定固定的样本时间下，高阶龙格-库塔方法提供了更高的精度，但代价是更多的计算成本 $${\mathrm{T.}}_{\mathrm{S.}}$$．

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

目录(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;结束功能DXDT = VDPSTATEFCNContinuous（X）％VDPSTATEFCNContion评估MU = 1 dxdt = [x（2）的范德波孔。（1-x（1）^ 2）* x（2）-x（1）];结尾

传感器建模

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

H(..)为测量函数，V.是测量噪声。你是可选的，并表示额外的输入H，例如系统输入或参数。U可以指定为零个或多个函数参数。您可以添加附加的系统输入你术语。这些输入可以不同于状态转移函数中的输入。

对于这个例子，假设你有第一状态的测量值 $$X_1$$在一定百分比误差范围内:

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

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

类型vdpMeasurementNonAdditiveNoiseFcn

示例测量函数用于含非加性测量噪声的离散%时间非线性状态估计器。% % 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);结尾

无创的卡尔曼滤波器建设

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

%您在时间k时的初始状态猜测，利用到时间k-1的测量:xhat[k|k-1]initialStateGuess = (2; 0);％xhat [k | k-1]％构造过滤器Ukf = UnscentedkalmanFilter（.．.@vdpStateFcn,.．.％状态转换功能@vdpMeasurementNonAdditiveNoiseFcn,.．.%测量功能我猜，.．.“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))));%根号(R):噪声标准差

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

nsteps = numel（ymeas）;%时间步长Xcorritedukf =零（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的测量值纳入状态估计值：%使用“correct”命令。这将更新State和StateCovariance%包含x[k | k]和P[k | k]的筛选器的属性。这些值%也会作为“correct”命令的输出产生。[xCorrectedUKF (k,:), PCorrected (k,:,:)] =正确(ukf, yMeas (k));%预测下一个时刻k+1的状态。这更新了国家和%包含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]);ylabel (“x_1”）;次要情节(2,1,2);绘图（TimeVector，Xtrue（:,2），TimeVector，XcorratedukF（:,2））;ylim (1.5 [3]);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 -√(PCorrected (:, 1, 1)),'r'）;%1-西格玛下界Xlabel（'时间[S]'）;ylabel ('状态1出错'）;标题('国家估计错误'）;次要情节(2,1,2);情节(timeVector地产(:,2),.．.第二个状态的错误timeVector,√PCorrected: 2 2)),'r'那.．.% 1-sigma上限timeVector -√(PCorrected (: 2 2)),'r'）;%1-西格玛下界Xlabel（'时间[S]'）;ylabel ('状态2出错'）;传奇(状态估计的那“1-sigma不确定性约束”那.．.“位置”那'最好的事物'）;

由于传感器模型(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-sigma不确定度范围的误差小于30%意味着良好的估计。这两种状态都满足该标准。第二种状态的0%百分比意味着滤波器是保守的：最有可能的是组合过程和测量噪声太高。可能是通过调整这些参数可以获得更好的性能。

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

意思是(地产)

ans =1×2-0.0103 - 0.0201

[xeStates1, xeStatesLags1] = xcorr(地产(:1),'coeff'）;％'coeff'：通过零滞后的值正常化[xeStates2, xeStatesLags2] = xcorr(地产(:,2),'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.结合估计状态准确的事实，残留物的这种行为可以被视为令人满意结果。

粒子滤波结构

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

这particleFilter控制系统工具箱中的命令实现了离散时间粒子滤波算法。本节将引导您完成构建particleFilter，并强调了它与无迹卡尔曼滤波器的异同。

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

类型vdpParticleFilterStateFcn

函数particles=vdpParticleFilterStateFcn（粒子）%vdpParticleFilterStateFcn mu=1的粒子%filter%%离散时间近似范德波尔方程的状态转移函数示例。%Sample time为0.05s.%predictedParticles=vdpParticleFilterStateFcn（粒子）%%输入：%particles-当前时间的粒子。维度为%[NumberOfState NumberOfParticles]的矩阵矩阵%%Outputs:%predictedParticles-下一时间步的预测粒子%%另见particleFilter%版权所有2017 MathWorks，Inc.%#codegen%如果您希望使用%MATLAB Coder生成代码，则必须包含标记%#codegen。[NumberOfState，numberOfParticles]=大小（粒子）；%时间传播每个粒子%%Euler连续时间动力学积分x'=f（x），采样时间dt=0.05；%s]采样时间kk=1:numberOfParticles粒子（：，kk）=粒子（：，kk）+vdpStateFcnContinuous（粒子（：，kk））*dt；结束%在每个状态变量processNoise=0.025上添加方差为0.025的高斯噪声（NumberOfState）；粒子=粒子+过程噪声*随机数（大小（粒子））；结束函数dxdt=VDPStatefCnConcontinuous（x）%VDPStatefCnConcontinuous评估mu=1的范德波尔方程dxdt=[x（2）；（1-x（1）^2）*x（2）-x（1）]；结束

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

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

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

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

pf.StateEstimationMethod

ans = '的意思是'

particleFilter让您指定各种重采样选项通过它ResamplingPolicy和重新制动方法属性。此示例使用过滤器中的默认设置。看到particleFilter有关重新采样的详细信息，请参阅文档。

pf.ResamplingMethod

ans = '多项'

pf.ResamplingPolicy

ans=particlesSamplingPolicy，属性为：TriggerMethod:'ratio'采样间隔：1 MinEffectiveParticleRatio:0.5000

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

%的估计xCorrectedPF = 0(大小(xTrue));为k = 1：尺寸（xtrue，1）%使用测量y[k]来修正时间k的粒子xCorrectedPF (k) =正确(pf, yMeas (k));%过滤器更新并存储粒子[k | k]，权重[k | k]结果是:在时间k时的状态估计，利用这个估计值是所有粒子的平均值%，因为statestimationmethod是mean。％现在，预测粒子在下一个时间步骤。这些在% next正确命令预测（PF）;% Filter更新和存储粒子[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, xCorrectedPF (:, 2));ylim (1.5 [3]);Xlabel（'时间[S]'）;ylabel (“x_2”）;

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

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

总结

本示例展示了构造和使用无迹卡尔曼滤波器和粒子滤波器对非线性系统进行状态估计的步骤。您通过噪声测量估计了范德波尔振荡器的状态，并验证了估计性能。

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