はじめに

控制系统工具箱™は,非線形の状態推定に関して3つのコマンドを提供します。

これらのコマンドの使用に关する一般的なワークフローは次のとおりです。

このこの例で，まずunscentedKalmanFilterコマンドを使用して,このワークフローについて説明します。その後,particleFilterの使用方法を説明します。

プラントのモデル化と離散化

アンセンテッドカルマンフィルター(UKF)アルゴリズムには,タイムステップ間の状態の変化を記述する関数が必要です。一般的にこれは状態遷移関数と呼ばれます。unscentedKalmanFilter は次の 2 つの関数形式をサポートします。

ここで,f(. .)は状態遷移関数,xは状態wはプロセスノイズです。uはオプションで,fに対する追加入力を表しています(たとえば,システム入力やパラメーター)。uには0個以上の関数引数を指定できます。加法性ノイズは,状態とプロセスノイズが線形的に関係していることを意味します。関係が非線形である場合は,2番目の形式を使用します。unscentedKalmanFilterオブジェクトを作成する場合,f（..）に加入て，プロセスプロセスが加法。

この例のシステムは,μ= 1のファンデルポール振動子です。この2状態システムは,次の非線形の常微分方程式(ODE)で記述されます。

この方程式を $$\underset{}{\overset{\dot{}}{x}}＝f_c（x）$$と表します。ここで, $$x＝［x_1；x_2］$$ですノイズWははシステムモデルは现れために.

unscentedKalmanFilterは離散時間状態遷移関数を必要としますが,プラントモデルは連続時間です。この連続時間モデルに離散時間近似を使用できます。オイラー法による離散化は一般的な近似手法の1つです。サンプル時間は $$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的范德堡尔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は次の2つの関数形式をサポートします。

h(. .)は測定関数,vは测定ノイズです。uはオプションで,hに対する追加入力を表しています(たとえば,システム入力やパラメーター)。uには0個以上の関数引数を指定できます。u項の後に追加のシステム入力を指定できます。これらの入力は,状態遷移関数の入力とは異なるものを指定できます。

この例では,最初の状態 $$x_1$$の測定の誤差が数パーセント以内であると仮定します。

測定ノイズが単に状態の関数に追加されることはないため,これは非加法性の測定ノイズのカテゴリに分類されます。ノイズを含む測定値から $$x_1$$と $$x_2$$の両方向推定します。

この状态迁移关键作作作作作者：vdpMeasurementNonAdditiveNoiseFcn.mという名前のファイルに保存します。オブジェクトの作成中,この関数をunscentedKalmanFilterに提供します。

类型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.プロパティを假に設定することで指定しなければなりません。この操作は,オブジェクトの作成中に行わなければなりません。アプリケーションの状態遷移関数に非加法性のプロセス ノイズがある場合、hasaddittyProcessnoise.プロパティを假に指定します。

%您在时间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.プロパティは,プロセスノイズの共分散を保存します。このプロパティは,モデルの不正確さや,未知の外乱がプラントに与える影響を説明するために設定されます。この例では実モデルを使用していますが,離散化によって誤差が発生します。単純化するために,この例には外乱が含まれていません。最初の状態ではノイズが少なく,2番目の状態ではノイズが増加している対角行列に設定することで,2番目の状態はモデル化誤差の影響を強く受けるという情報を反映させます。

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

正确预测コマンドとコマンドを使用した推定

アプリケーションでは，ハードウェアからリアルタイムででした测定データががによって逐次さでれれこのこのではますはれれはますははははデータセットをしにに测定データデータをごとフィルター次にそのデータデータをごとフィルターに送ることことことことことことことでことことことででことことことことで说明します。

フィルターのサンプル時間を0.05 [s]に設定し,ファン・デル・ポール振動子を5秒間シミュレートすることで,システムの実際の状態を生成します。

T = 0.05;% [s]过滤采样时间TimeVector = 0：T：5;[〜，xtrue] = ode45（@ VDP1，TimeVector，[2; 0]）;

各測定における標準偏差を45%の誤差とし,センサーが最初の状態を測定すると仮定して測定値を生成します。

rng (1);%修复随机数生成器的可重复结果ytrue = xtrue（：，1）;YMEAS = YTRUE。*（1 + SQRT（R）* RANDN（尺寸（ytrue）））;%根号(R):噪声标准差

後で解析するデータ用の空間を事前に割り当てます。

Nsteps =元素个数(yMeas);％步骤数xCorrectedUKF = 0 (Nsteps, 2);修正状态估计值pcorrited = zeros（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]。这些值%也会作为“correct”命令的输出产生。[Xcorratedukf（k，:)，pcorrited（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'）;

上のプロットは,最初の状態に関する真値,推定された値,および測定値を示しています。フィルターはシステムモデルとノイズ共分散に関する情報を利用し,測定値よりも改善された推定値を生成します。下のプロットは2番目の状態を示しています。フィルターは適切な推定を正しく生成します。

アンセンテッドまたは拡張カルマンフィルターのパフォーマンス検証は,通常は幅広いモンテカルロ法シミュレーションを使用して実行されます。こうしたシミュレーションでは,次のバリエーションがテストされます。プロセスノイズと測定ノイズの実現,さまざまな条件下でのプラント操作,初期状態および状態共分散の推定。状態推定の検証に使用される主な対象信号は残差(つまりイノベーション)です。この例では,単一のシミュレーションに対して残差分析を実行します。残差をプロットします。

图();绘图（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\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，SQRT（Pcorrited（：，2,2）），“r”，......％1-sigma上限TimeVector，-SQRT（Pcorrited（:,2,2）），“r”）;% 1-sigma下界包含(“时间[s]”）;ylabel ('状态2出错'）;传奇('国家估计'，“1-sigma不确定性约束”，......'地点'，“最佳”）;

センサーモデル（MeasurementFcn）なので，t = 2.15秒での状态1の误差范囲0に近づきます。 $$x_1［k］＊（1+v［k］）$$です.t = 2.15秒では,実際の状態および推定された状態は0に近くなっています。これは,絶対項での測定値誤差も0に近いことを示しています。これはフィルターの状態推定誤差の共分散に反映されます。

何パーセントの点が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×20.1386 0.

最初のの状态推定推定の误差，そのタイムステップにおける $$1\sigma$$の不確かさ範囲である約14%を超えています。1シグマの不確かさ範囲を超えている誤差の 30% 未満が、適切な推定であることを示します。この条件は両方の状態で満たされます。2 番目の状態に対する 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よりもはるかに小さいものです。推定された状態は正確であるという事実と組み合わせることで,残差に関するこの動作は満足できる結果であると考えることができます。

粒子フィルターの构筑

�あり，アプリケーションによっては，状态推定値の事后事后の平均と共ををするだけでははするするだけで不ははですするだけでははですですするフィルターでははですするだけコストでははますが计算コスト高度なりが，多重の状态仮なりますが，多重の状态说（粒子）のの时的な子の问题をにより，この子の问题をます。

控制系统工具箱のparticleFilterコマンドは,離散時間の粒子フィルターアルゴリズムを実装します。この節では,この例で前に使用したのと同じファンデルポール振動子のparticleFilterを構築する手順を示し,アンセンテッドカルマンフィルターとの類似点および相違点について説明します。

particleFilterに与える状態遷移関数は,次の2つのタスクを実行しなければなりません。1。システムに適した分布からのプロセスノイズのサンプリング。2。すべての粒子(状態仮説)の,次のステップまでの時間伝播の計算。これにはステップ1で計算したプロセスノイズの効果も含みます。

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

unscentedKalmanFilterとparticleFilterでは指定する状态迁移关键词异なります。アンセンテッドカルマンフィルターに使使しした迁移た状态迁移さらにステップの状态仮说の次タイムステップの伝播伝播さらにするものへのさらにものものものものものものものものものさらにものものものものものものものものものものものものものものものものものものものものものものものものさらにものものものものものものものものものさらにものものものものものさらにさらにさらにlプロセスノイズは，その共分享のみによってunscentedKalmanFilterのprocessnoise.プロパティで定义されていました。particleFilterは，さらに很多の统计プロパティ定义の必要がある可口がある任意ののの考虑するますますこの考虑ことができます。particleFilterに指定する状态迁移关数内で完全に定义されます。

particleFilterに指定する测定尤度关键，次の2つのタスクを実しなけれなりません.1 .1。粒子からの测定仮说の计算.2。センサー测定値およびステップ1で计算した仮说からの各粒子の尤度の计算。

类型VDPEXAMPLEPFMEASURENCELIKELIOOWFCN.

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

次次にフィルターを构筑し，平面[2;0]の周りに0.01の共共共もつ1000个のの子ににしします。0]

pf = particlefilter（@ vdpparticlefilterstatefcn，@ vdpexamplepfmeasurementlikelihoodfcn）;初始化（PF，1000，[2; 0]，0.01 *眼睛（2））;

オプションで,状態推定法を選択します。これはparticleFilterの最终的方法プロパティによって設定され,'意思'(既定の設定)または“maxweight”の値をとることができます。最终的方法が'意思'の場合,オブジェクトは状態推定値として粒子プロパティと权重から粒子の加重平均を抽出します。“maxweight”は权重のの最说推定値として粒(を选択することことにししししし选択し状态するするするするししあるいは选択するすることことししし状态するするするするますします状态状态状态する推定するするするます状态状态状态粒子プロパティと权重プロパティにアクセスして,任意の手法を選択して状態推定値を抽出することもできます。

pf.StateEstimationMethod

ans = '的意思是'

particleFilterでは，そのResamplingPolicyプロパティとResamplingMethodプロパティを使用してさまざまなリサンプリングオプションを指定できます。この例はフィルターの既定の設定を使用します。リサンプリングの詳細については,particleFilterのドキュメンテーションを参照してください。

pf.resamplingmethod.

ans ='多项式'

pf.ResamplingPolicy

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

推定ループを开始します。これは时间の経过とともにステップごとに取得される测定値を表します。

%的估计xcorrectpf = zeros（尺寸（xtrue））;为k = 1:尺寸(xTrue, 1)%使用测量y[k]来修正时间k的粒子xcorrectpf（k，:) =正确（pf，Ymeas（k））;% Filter更新和存储粒子[k|k]，权重[k|k]％结果是x [k | k]：利用时，估计状态k，利用％测量到时间k。这种估计是所有粒子的含义％因为enveryestimationMethod是“意思”。％现在，预测粒子在下一个时间步骤。这些在% 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]);包含(“时间[s]”）;ylabel ('x_2'）;

上のプロットは,最初の状態の真値,粒子フィルターの推定値,および測定値を示しています。フィルターはシステムモデルとノイズ情報を利用し,測定値よりも改善された推定値を生成します。下のプロットは2番目の状態を示しています。フィルターは適切な推定を正しく生成します。

粒子フィルターの性能の検証には,前のアンセンテッドカルマンフィルターの結果の例で実行したような,残差の統計検定の実行が含まれます。

まとめ

この例では，非线形システムの状态推定値に対するアンセンテッドカルマンフィルターおよび粒子フィルターを构筑して使用する手顺を示しました。ノイズを含む测定値からファン·デル·ポール振动子の状态を推定しました。また，推定のパフォーマンスを検证したた。

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