介绍

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

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

这个例子首先使用unscentedKalmanFilter命令来演示此工作流。然后演示了particleFilter的使用。

植物建模与离散化

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

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

这个例子中的系统是=1的范德堡尔振荡器。该2态系统可用以下一组非线性常微分方程(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在对象构造过程中。

目录(fullfile (matlabroot,“例子”，“控制”，“主要”））添加示例数据类型vdpStateFcn

函数x = vdpStateFcn(x) % vdpStateFcn mu = 1时van der Pol ode的离散时间近似。%采样时间为0.05s。离散非线性状态估计器的状态转移函数示例。% % xk1 = vdpStateFcn(xk) % %输入:% xk -状态x[k] % %输出:% xk1 -传播状态x[k+1] % %另见extendedKalmanFilter, unscentedKalmanFilter %版权2016 The MathWorks, Inc. %#codegen %如果您希望使用% MATLAB Coder生成代码，则必须包含标签%#codegen。%连续时间动力学的欧拉积分x'=f(x)与样本时间dt dt = 0.05;% [s]采样时间x = x + vdpStateFcnContinuous(x)*dt;计算mu = 1的范德波尔ode值dxdt = [x(2);(1 - x (1) ^ 2) * (2) x - x (1)];结束

传感器建模

unscentedKalmanFilter还需要一个函数来描述模型状态如何与传感器测量相关。unscentedKalmanFilter支持以下两种函数形式:

h(..)为测量函数，v是测量噪声。u是可选的，并表示对h，例如系统输入或参数。U可以指定为0个或多个函数参数。方法之后可以添加其他系统输入u术语。这些输入可以不同于状态转换函数中的输入。

对于这个例子，假设您已经测量了第一个状态 $$x_1$$在一定百分比误差范围内:

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

创建这个状态转换函数，并将其保存在一个名为vdpMeasurementNonAdditiveNoiseFcn.m。函数提供此函数unscentedKalmanFilter在对象构造过程中。

类型vdpMeasurementNonAdditiveNoiseFcn

函数yk = vdpMeasurementNonAdditiveNoiseFcn(xk,vk) % 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,…测量函数initialStateGuess,…“HasAdditiveMeasurementNoise”、假);

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

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

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

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

使用预测和正确的命令

在应用程序中，来自硬件的实时测量数据在到达时由过滤器处理。在本例中，通过首先生成一组测量数据，然后将其一次一步地提供给过滤器来演示此操作。

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

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

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

rng (1);修复随机数生成器的可再现结果yTrue = xTrue(:，1);yMeas = yTrue .*(1+√(R)*randn(size(yTrue)));%根号(R):噪声的标准差

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

Nsteps = nummel (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时刻的测量值合并到状态估计中%使用"correct"命令。这将更新State和StateCovariance%属性的过滤器包含x[k|k]和P[k|k]。这些值%也会作为"correct"命令的输出产生。[xCorrectedUKF (k,:), PCorrected (k,:,:)] =正确(ukf, yMeas (k));预测下一个时间步k+1的状态。这将更新State和% 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]);ylabel (“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\sigma$$不确定性边界来自滤波误差协方差估计。

eStates = xTrue-xCorrectedUKF;图();次要情节(2,1,1);情节(timeVector地产(:1),…%第一个状态的错误timeVector,√PCorrected: 1 1)),“r”，…% 1-sigma上限timeVector -√(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-西格玛不确定性界限”，…“位置”，“最佳”）;

状态1的误差界限在t=2.15秒时接近0，这是因为传感器模型(MeasurementFcn）.它有这样的形式 $$x_1［k］*（1+v［k］）$$。在t=2.15秒时，真实状态和估计状态接近于零，这意味着测量误差绝对值也接近于零。这反映在滤波器的状态估计误差协方差上。

计算超出1-sigma不确定界限的点的百分比。

distanceFromBound1 = abs(eStates(:，1))-sqrt(PCorrected(:，1,1));percentageexcepded1 = nnz(distanceFromBound1>0) / nummel (eStates(:，1));distanceFromBound2 = abs(eStates(:，2))-sqrt(PCorrected(:，2,2));percentageexcepded2 = nnz(distanceFromBound2>0) / nummel (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(eStates(:，1)，多项式系数的）;% 'coeff':由零延迟时的值规范化[xeStates2,xeStatesLags2] = xcorr(eStates(:，2)，多项式系数的）;%的多项式系数%仅绘制非负滞后idx = xeStatesLags1>=0;图();次要情节(2,1,1);情节(xeStatesLags1 (idx) xeStates1 (idx));包含(“滞后”）;ylabel (“对于状态1”）;标题(状态估计误差的归一化自相关）;次要情节(2,1,2);情节(xeStatesLags2 (idx) xeStates2 (idx));包含(“滞后”）;ylabel (“对于状态2”）;

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

粒子滤波结构

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

的particleFilter命令实现了一个离散粒子滤波算法。本节将引导您构造一个particleFilter对于本例中前面使用的同一个范德堡尔振荡器，并强调了与unscented卡尔曼滤波器的相似点和不同点。

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

类型vdpParticleFilterStateFcn

函数粒子= vdpParticleFilterStateFcn(粒子)% vdpParticleFilterStateFcn粒子的示例状态转换函数% filter % % mu = 1时范德波尔ode的离散时间近似。%采样时间为0.05s。% % predictedParticles = vdpParticleFilterStateFcn(particles) % %输入:% particles -当前时间的粒子数。矩阵与维度% [NumberOfStates NumberOfParticles]矩阵% %输出:% predictedParticles -预测粒子下一个时间步骤% %参见particleFilter %版权2017 the MathWorks, Inc. %#codegen %如果您希望使用% MATLAB Coder生成代码，必须包括标签%#codegen。[numberOfStates, numberOfParticles] = size(particles);%时间传播每个粒子% %连续时间动力学的欧拉积分x'=f(x)与样本时间dt dt = 0.05;% [s]采样时间为kk=1:numberOfParticles粒子(:，kk) =粒子(:，kk) + vdpStateFcnContinuous(粒子(:，kk))*dt;对每个状态变量processNoise = 0.025*eye(numberOfStates)添加方差为0.025的高斯噪声;粒子=粒子+ processNoise * randn(大小(粒子));计算mu = 1的范德波尔ode值dxdt = [x(2); (1-x(1)^2)*x(2)-x(1)]; end

您提供给的状态转换函数之间存在差异unscentedKalmanFilter和particleFilter。你用于无味卡尔曼滤波的状态转移函数只是描述了一个状态假设到下一个时间步的传播，而不是一组假设。在此基础上，定义了过程噪声分布ProcessNoise的属性unscentedKalmanFilter，就是它的协方差。particleFilter可以考虑可能需要定义更多统计属性的任意分布。这种任意分布及其参数在您提供给的状态转换函数中完全定义particleFilter。

你提供给的测量似然函数particleFilter还必须执行两项任务。第一，从粒子中计算测量假设。第二，从传感器测量和第一步中计算的假设中计算每个粒子的可能性。

类型vdpExamplePFMeasurementLikelihoodFcn

function似然= vdpexamplepfmeasurementlikehoodfcn(粒子，测量)% vdpexamplepfmeasurementlikehoodfcn示例测量似然函数% %测量为第一状态。% % = vdpParticleFilterMeasurementLikelihoodFcn可能性(粒子,测量)输入% %:%粒子——NumberOfStates-by-NumberOfParticles矩阵包含%粒子% %输出:%的可能性——一个向量与NumberOfParticles元素n %元素是第n个粒子% %的可能性也看到extendedKalmanFilter unscentedKalmanFilter % 2017年版权MathWorks公司% # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。验证传感器测量数ofmeasurements = 1;%期望测量的数量验证属性(measurement， {'double'}， {'vector'， 'numel'， numberOfMeasurements}，…“vdpExamplePFMeasurementLikelihoodFcn”、“测量”);测量是第一状态。从粒子predictedMeasurement =粒子(1，:)中获取所有测量假设;假设预测与实际测量之间的误差比例%遵循均值为零的高斯分布，方差为0.2 mu = 0;% mean sigma = 0.2 * eye(numberOfMeasurements);使用多元高斯概率密度函数，计算每个粒子的%概率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

现在构造过滤器，并在平均值[2;0]，协方差0.01。协方差很小，因为你对你的猜测有很高的信心[2;0]。

pf = particleFilter(@vdpParticleFilterStateFcn，@ vdpexamplepfmeasurementlikehoodfcn);初始化(pf, 1000， [2;0]， 0.01*eye(2));

可选地，选择状态估计方法。这是由StateEstimationMethod的属性particleFilter，可取值“的意思是”(默认)或“maxweight”。当StateEstimationMethod是“的意思是”时，对象提取粒子的加权平均值粒子和权重属性作为状态估计。“maxweight”对应于选择中权重值最大的粒子(状态假设)权重正如国家估计的那样。或者，您可以访问粒子和权重属性，并通过您选择的任意方法提取您的状态估计。

pf.StateEstimationMethod

Ans =“刻薄的”

particleFilter允许您通过its指定各种重新采样选项ResamplingPolicy和ResamplingMethod属性。本例使用筛选器中的默认设置。看到particleFilter关于重新采样的进一步详细信息的文档。

pf.ResamplingMethod

Ans = '多项的'

pf.ResamplingPolicy

ans = particleResamplingPolicy with properties: TriggerMethod: 'ratio' 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时刻的状态估计，利用%的测量到时间k。这个估计值是所有粒子的平均值%因为StateEstimationMethod是'mean'。％现在，预测下一阶段的粒子。它们被用于%下一个正确命令预测(pf);%过滤器更新和存储粒子[k+1|k]结束

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

图();次要情节(2,1,1);情节(timeVector xTrue (: 1), timeVector, xCorrectedPF (: 1), timeVector, yMeas (:));传奇(“真正的”，“粒子滤波估计”，“测量”) ylim([-2.6 2.6]);ylabel (“x_1”）;次要情节(2,1,2);情节(timeVector xTrue (:, 2), timeVector, xCorrectedPF (:, 2));ylim (1.5 [3]);包含(“时间[s]”）;ylabel (“x_2”）;

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

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

总结

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

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