介绍

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

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

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

植物建模和离散化

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

在这里f(. .)是状态转换函数,x是国家,w是噪音。你是可选的,表示额外的输入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在对象构造。

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

函数x = vdpStateFcn (x) % vdpStateFcn离散近似为μ= 1范德堡尔常微分方程。%样本时间是0.05秒。% % %离散时间非线性状态估计状态转换函数例子。% % % % xk1 = vdpStateFcn (xk)输入:% xk -州x [k] % %输出:% xk1 -传播国家x [k + 1] % %也看到extendedKalmanFilter unscentedKalmanFilter % 2016年版权MathWorks公司% # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。%欧拉积分的连续时间动态x ' = f (x)与样本时间dt dt = 0.05;% [s]样品时间x = x + vdpStateFcnContinuous (x) * dt;结束函数dxdt = vdpStateFcnContinuous (x) % vdpStateFcnContinuous评估范德堡尔常微分方程为μ= 1 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在对象构造。

类型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。MeasurementNoise = R;

的ProcessNoise属性存储过程噪声协方差。它将占模型不准确和未知扰动的影响。在这个例子中我们有真正的模型,但离散化引入了错误。这个例子不包括任何干扰为简单起见。将其设置为一个对角矩阵第一状态,用更少的噪音和第二状态反映的知识第二状态影响建模错误。

ukf。ProcessNoise =诊断接头([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 +√(R) * randn(大小(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时刻测量纳入国家的估计%使用“正确”命令。这更新状态和StateCovariance%的属性过滤器包含x k | k, P (k | k)。这些值%也产生“正确”命令的输出。[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]);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,多项式系数的);%的多项式系数:在零延迟的值正常化%只有情节非负落后idx = xeLags > = 0;图();情节(xeLags (idx), xe (idx));包含(“滞后”);ylabel (归一化相关的);标题(残差的自相关(创新));

除了0所有滞后的相关性很小。相关性是接近于零,没有任何明显的随机变化的相关性。这些特征增加信心,滤波器的性能。

在现实中真正的国家从来都不是可用的。然而,当执行模拟,你获得真正的州和可以看看之间的错误估计和真正的状态。这些错误必须满足类似的残留标准:

首先,看错误, $$1\sigma$$滤波误差协方差估计的不确定性范围。

地产= 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-sigma不确定性约束”,…“位置”,“最佳”);

错误开往状态1趋于0 t = 2.15秒,因为传感器模型(MeasurementFcn)。它的形式 $$x_1(k]*(1+v(k])$$。在t = 2.15秒的真实和估计状态接近于零,这意味着测量误差的绝对值也接近于零。这反映在状态估计误差协方差的过滤器。

计算的百分比点超出了1-sigma不确定性约束。

distanceFromBound1 = abs(地产(:1))-√(PCorrected (:, 1, 1));percentageExceeded1 = nnz (distanceFromBound1 > 0) /元素个数(地产(:1));distanceFromBound2 = abs(地产(:,2))-√(PCorrected (:, 2, 2));percentageExceeded2 = nnz (distanceFromBound2 > 0) /元素个数(地产(:,2));[percentageExceeded1 percentageExceeded2]

ans =1×20.1386 0

第一个状态估计误差超过 $$1\sigma$$不确定性约束大约14%的时间步骤。不到30%的错误超过1-sigma不确定性约束意味着良好的估计。这一标准为两国感到满意。0%比例的第二状态意味着过滤器是保守:最有可能结合过程和测量噪音太高了。可能是通过调优这些参数可以获得更好的性能。

计算的平均自相关状态估计错误。还绘制自相关。

意思是(地产)

ans =1×2-0.0103 - 0.0201

[xeStates1, xeStatesLags1] = xcorr(地产(:1),多项式系数的);%的多项式系数:在零延迟的值正常化[xeStates2, xeStatesLags2] = xcorr(地产(:,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同样的范德堡尔振荡器使用之前在这个例子中,并强调与无味卡尔曼滤波的异同。

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

类型vdpParticleFilterStateFcn

函数= vdpParticleFilterStateFcn粒子(粒子)% vdpParticleFilterStateFcn例子粒子状态转换函数%过滤器% %离散近似为μ= 1范德堡尔常微分方程。%样本时间是0.05秒。% % % % predictedParticles = vdpParticleFilterStateFcn(粒子)输入:%粒子-粒子在当前时间。矩阵维度% [NumberOfStates NumberOfParticles]矩阵输出% %:% predictedParticles——预测粒子为下一个时间步% %参见particleFilter % 2017年版权MathWorks, Inc . % # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。[numberOfStates, numberOfParticles] =大小(颗粒);% Time-propagate每个粒子% %欧拉积分的连续时间动态x ' = f (x)与样本时间dt dt = 0.05;% [s]样品时间kk = 1: numberOfParticles粒子(:,kk) =粒子(:,kk) + vdpStateFcnContinuous dt(粒子(:,kk)) *;结束%添加高斯噪声方差0.025在每个状态变量processNoise = 0.025 *眼(numberOfStates);粒子=粒子+ processNoise * randn(大小(粒子));结束函数dxdt = vdpStateFcnContinuous (x) % vdpStateFcnContinuous评估范德堡尔常微分方程为μ= 1 dxdt = [x (2); (1-x(1)^2)*x(2)-x(1)]; end

有你供应的状态转换函数之间的区别unscentedKalmanFilter和particleFilter。用于无味卡尔曼滤波器的状态转换函数只是描述一种状态假设的传播到下一个时间步长,而不是一组假设。此外,流程中定义的噪声分布ProcessNoise财产的unscentedKalmanFilter,只要其协方差。particleFilter可以考虑任意分布,可能需要更多的统计特性的定义。这个任意分布及其参数完全定义在您提供的状态转换函数particleFilter。

您提供的测量似然函数particleFilter还必须执行两个任务。1、计算测量从粒子假说。2,计算每个粒子的可能性从传感器测量和假设计算第一步。

类型vdpExamplePFMeasurementLikelihoodFcn

函数= vdpExamplePFMeasurementLikelihoodFcn可能性(粒子,测量)测量似然函数% % % vdpExamplePFMeasurementLikelihoodFcn例子是第一个状态。% % = vdpParticleFilterMeasurementLikelihoodFcn可能性(粒子,测量)输入% %:%粒子——NumberOfStates-by-NumberOfParticles矩阵包含%粒子% %输出:%的可能性——一个向量与NumberOfParticles元素n %元素是第n个粒子% %的可能性也看到extendedKalmanFilter unscentedKalmanFilter % 2017年版权MathWorks公司% # % # codegen %标签codegen必须包括如果你想用% MATLAB编码器生成代码。%验证传感器测量numberOfMeasurements = 1;% validateattributes预期数量的测量(测量、{“双重”},{“向量”、“元素个数”numberOfMeasurements},…“vdpExamplePFMeasurementLikelihoodFcn”、“测量”);%的测量是第一个状态。得到所有测量假设从粒子predictedMeasurement =粒子(1:);%假设误差预测和实际测量值之间的比例%遵循与零均值高斯分布,方差0.2μ= 0;%的意思是σ= 0.2 *眼(numberOfMeasurements);%方差%使用多元高斯概率密度函数,计算每个粒子的可能性% numParticles =大小(粒子,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.01 0]。协方差小,因为你有很高的信心在你的猜测(2;0]。

pf = particleFilter (@vdpParticleFilterStateFcn @vdpExamplePFMeasurementLikelihoodFcn);初始化(pf, 1000,(2, 0), 0.01 *眼(2));

可选地,选择状态估计方法。这是规定的StateEstimationMethod的属性particleFilter,这可能需要值“的意思是”(默认)或“maxweight”。当StateEstimationMethod是“的意思是”,对象提取的粒子的加权平均数粒子和权重属性的状态估计。“maxweight”对应于选择最大重量的粒子(状态假设)的价值权重状态估计。或者,您可以访问粒子和权重对象的属性和提取你的状态估计通过任意方法的选择。

pf.StateEstimationMethod

ans = '的意思是'

particleFilter通过允许您指定不同重采样选项ResamplingPolicy和ResamplingMethod属性。这个示例使用的默认设置过滤器。看到particleFilter文档重新采样的进一步详情。

pf.ResamplingMethod

ans = '多项'

pf.ResamplingPolicy

ans = particleResamplingPolicy属性:TriggerMethod:‘比’SamplingInterval: 1 MinEffectiveParticleRatio: 0.5000

启动估计循环。这是测量到随着时间的推移,一步一步。

%的估计xCorrectedPF = 0(大小(xTrue));为k = 1:尺寸(xTrue, 1)%使用正确测量y [k]粒子时间kxCorrectedPF (k) =正确(pf, yMeas (k));%过滤器更新和存储粒子(k | k),重量(k | k)%结果x [k | k]:估计k,州的时间利用%测量k。这个估计是所有粒子的均值%因为StateEstimationMethod“的意思是”。%%,下次一步预测粒子。这些都是利用%的下一个正确的命令预测(pf);%过滤器更新和存储粒子(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]);包含(“时间[s]”);ylabel (“x_2”);

顶部情节展示了真实价值,粒子滤波估计,第一个状态的测量值。过滤器利用系统模型和噪声信息产生一种改进的测量估计。底部图显示了第二个国家。筛选成功的生产是一个很好的估计。

粒子滤波性能的验证包括对残差进行统计检验,类似于那些在本例中为早些时候进行无味卡尔曼滤波的结果。

总结

这个例子显示的步骤构建和使用无味卡尔曼滤波和粒子滤波的非线性系统的状态估计。你估计状态的范德堡尔振荡器噪声测量和验证估计的性能。

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