ドキュメンテーション

最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

アンセンテッド カルマン フィルターを使用した非線形の状態推定

この例では、ファン デル ポール振動子の非線形の状態推定に対して、アンセンテッド カルマン フィルター アルゴリズムを使用する方法を説明します。

この例ではさらに、Signal Processing Toolbox™ を使用します。

はじめに

Control System Toolbox™ は、非線形の状態推定に関して 2 つのコマンドを提供します。

  • extendedKalmanFilter: 1 次離散時間の拡張カルマン フィルター

  • unscentedKalmanFilter: 離散時間のアンセンテッド カルマン フィルター

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

  1. プラントとセンサーの動作をモデル化します。

  2. extendedKalmanFilter オブジェクトまたは unscentedKalmanFilter オブジェクトを作成および構成します。

  3. このオブジェクトを指定して predict コマンドと correct コマンドを使用し、状態推定を実行します。

  4. 結果を解析し、フィルターのパフォーマンスを確認します。

  5. ハードウェアにフィルターを展開します。MATLAB Coder™ を使用すると、これらのフィルターのコードを生成できます。

この例では、このワークフローについて説明するために unscentedKalmanFilter コマンドを使用します。

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

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

  1. 加法性のプロセス ノイズ:

  2. 非加法性のプロセス ノイズ:

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

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

この方程式を と表します。ここでは、 です。プロセス ノイズ w はシステム モデルには現れません。単純化するために、このプロセス ノイズは加法性であると仮定します。

unscentedKalmanFilter は離散時間状態遷移関数を必要としますが、プラント モデルは連続時間です。この連続時間モデルに離散時間近似を使用できます。オイラー法による離散化は一般的な近似手法の 1 つです。サンプル時間は であると仮定し、連続時間のダイナミクスを として表します。オイラー法による離散化では、微分演算子を として近似します。結果の離散時間状態遷移関数は次のようになります。

この近似の精度は、サンプル時間 によって変わります。 の値が小さければ、近似の精度が高まります。また、異なる離散化手法を使用することもできます。たとえば、ルンゲ・クッタ法は、固定のサンプル時間 が与えられた場合、次数が高いほど、計算コストは高くなりますが、より高い精度を実現できます。

この状態遷移関数を作成し、vdpStateFcn.m という名前のファイルに保存します。サンプル時間 を使用します。オブジェクトの作成中、この関数を unscentedKalmanFilter に提供します。

type vdpStateFcn
function x = vdpStateFcn(x) 
% vdpStateFcn Discrete-time approximation to van der Pol ODEs for mu = 1. 
% Sample time is 0.05s.
%
% Example state transition function for discrete-time nonlinear state
% estimators.
%
% xk1 = vdpStateFcn(xk)
%
% Inputs:
%    xk - States x[k]
%
% Outputs:
%    xk1 - Propagated states x[k+1]
%
% See also extendedKalmanFilter, unscentedKalmanFilter

%   Copyright 2016 The MathWorks, Inc.

%#codegen

% The tag %#codegen must be included if you wish to generate code with 
% MATLAB Coder.

% Euler integration of continuous-time dynamics x'=f(x) with sample time dt
dt = 0.05; % [s] Sample time
x = x + vdpStateFcnContinuous(x)*dt;
end

function dxdt = vdpStateFcnContinuous(x)
%vdpStateFcnContinuous Evaluate the van der Pol ODEs for mu = 1
dxdt = [x(2); (1-x(1)^2)*x(2)-x(1)];
end

センサー モデリング

unscentedKalmanFilter には、モデルの状態がセンサー測定値にどのように関係しているかを記述する関数も必要です。unscentedKalmanFilter は次の 2 つの関数形式をサポートします。

  1. 加法性の測定ノイズ:

  2. 非加法性の測定ノイズ:

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

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

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

この状態遷移関数を作成し、vdpMeasurementNonAdditiveNoiseFcn.m という名前のファイルに保存します。オブジェクトの作成中、この関数を unscentedKalmanFilter に提供します。

type vdpMeasurementNonAdditiveNoiseFcn
function yk = vdpMeasurementNonAdditiveNoiseFcn(xk,vk)
% vdpMeasurementNonAdditiveNoiseFcn Example measurement function for discrete
% time nonlinear state estimators with non-additive measurement noise.
%
% yk = vdpNonAdditiveMeasurementFcn(xk,vk)
%
% Inputs:
%    xk - x[k], states at time k
%    vk - v[k], measurement noise at time k
%
% Outputs:
%    yk - y[k], measurements at time k
%
% The measurement is the first state with multiplicative noise
%
% See also extendedKalmanFilter, unscentedKalmanFilter

%   Copyright 2016 The MathWorks, Inc.

%#codegen

% The tag %#codegen must be included if you wish to generate code with 
% MATLAB Coder.

yk = xk(1)*(1+vk);
end

フィルター構築

状態遷移関数および測定関数に対する関数ハンドル、次に初期状態の推定値を指定することで、フィルターを構築します。状態遷移モデルには加法性ノイズがあります。これはフィルターの既定の設定であるため、指定する必要はありません。測定モデルには非加法性ノイズがあり、オブジェクトの HasAdditiveMeasurementNoise プロパティを false に設定することで指定しなければなりません。この操作は、オブジェクトの作成中に行わなければなりません。アプリケーションの状態遷移関数に非加法性のプロセス ノイズがある場合、HasAdditiveProcessNoise プロパティを false に指定します。

% Your initial state guess at time k, utilizing measurements up to time k-1: xhat[k|k-1]
initialStateGuess = [2;0]; % xhat[k|k-1]
% Construct the filter
ukf = unscentedKalmanFilter(...
    @vdpStateFcn,... % State transition function
    @vdpMeasurementNonAdditiveNoiseFcn,... % Measurement function
    initialStateGuess,...
    'HasAdditiveMeasurementNoise',false);

測定ノイズの共分散に関する情報を指定します。

R = 0.2; % Variance of the measurement noise v[k]
ukf.MeasurementNoise = R;

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

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

predict コマンドと correct コマンドを使用した推定

アプリケーションでは、ハードウェアからリアルタイムで受信した測定データがフィルターによって逐次処理されます。この例では、最初に測定データセットを生成し、次にそのデータをステップごとにフィルターに送ることで、この動作について説明します。

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

T = 0.05; % [s] Filter sample time
timeVector = 0:T:5;
[~,xTrue]=ode45(@vdp1,timeVector,[2;0]);

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

rng(1); % Fix the random number generator for reproducible results
yTrue = xTrue(:,1);
yMeas = yTrue .* (1+sqrt(R)*randn(size(yTrue))); % sqrt(R): Standard deviation of noise

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

Nsteps = numel(yMeas); % Number of time-steps
xCorrected = zeros(Nsteps,2); % Corrected state estimates
PCorrected = zeros(Nsteps,2,2); % Corrected state estimation error covariances
e = zeros(Nsteps,1); % Residuals (or innovations)

correct コマンドと predict コマンドを使用して、状態 x のオンライン推定を実行します。生成されたデータをタイム ステップごとにフィルターに提供します。

for k=1:Nsteps
    % Let k denote the current time
    %
    % Residuals (or innovations): Measured output - Predicted output
    e(k) = yMeas(k) - vdpMeasurementFcn(ukf.State); % ukf.State is x[k|k-1] at this point
    % Incorporate the measurements at time k into the state estimates by
    % using the "correct" command. This updates the State and StateCovariance
    % properties of the filter to contain x[k|k] and P[k|k]. These values
    % are also produced as the output of the "correct" command.
    [xCorrected(k,:), PCorrected(k,:,:)] = correct(ukf,yMeas(k));
    % Predict the states at next time step, k+1. This updates the State and
    % StateCovariance properties of the filter to contain x[k+1|k] and
    % P[k+1|k]. These will be utilized by the filter at the next time-step
    predict(ukf);
end

結果と検証

時間の経過に合わせて実際の状態と推定される状態をプロットします。また、最初の状態の測定値もプロットします。

figure();
subplot(2,1,1);
plot(timeVector,xTrue(:,1),timeVector,xCorrected(:,1),timeVector,yMeas(:));
legend('True','Filter estimate','Measured')
ylim([-2.6 2.6]);
ylabel('x_1');
subplot(2,1,2);
plot(timeVector,xTrue(:,2),timeVector,xCorrected(:,2));
ylim([-3 1.5]);
xlabel('Time [s]');
ylabel('x_2');

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

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

figure();
plot(timeVector, e);
xlabel('Time [s]');
ylabel('Residual (or innovation)');

残差は次のとおりとします。

  1. 小さい振幅

  2. ゼロ平均

  3. 自己相関なし (ラグ 0 を除く)

残差の平均値は次のとおりです。

mean(e)
ans =

   -0.0012

これは、残差の振幅に比べて小さい値です。残差の自己相関は、Signal Processing Toolbox の関数 xcorr を使用して計算できます。

[xe,xeLags] = xcorr(e,'coeff'); % 'coeff': normalize by the value at zero lag
% Only plot non-negative lags
idx = xeLags>=0;
figure();
plot(xeLags(idx),xe(idx));
xlabel('Lags');
ylabel('Normalized correlation');
title('Auto-correlation of residuals (innovation)');

相関は、0 を除くすべてのラグで小さくなっています。相関の平均は 0 に近く、相関にはランダムでない大きな変化はありません。こうした特性はフィルター パフォーマンスの信頼度を向上させます。

現実には、実際の状態を見ることはできません。しかし、追加の正常性チェックとして、実際の状態にアクセスできるシミュレーションを実行すると、推定された状態と実際の状態の間の誤差を確認できます。こうした誤差は、次のような残差に対する同様の条件を満たさなければなりません。

  1. 小さい振幅

  2. 分散がフィルター誤差の共分散推定の範囲内にある

  3. ゼロ平均

  4. 相関なし

最初に、フィルター誤差の共分散推定から、誤差範囲と の不確実性範囲を確認します。

eStates = xTrue-xCorrected;
figure();
subplot(2,1,1);
plot(timeVector,eStates(:,1),...               % Error for the first state
    timeVector, sqrt(PCorrected(:,1,1)),'r', ... % 1-sigma upper-bound
    timeVector, -sqrt(PCorrected(:,1,1)),'r');   % 1-sigma lower-bound
xlabel('Time [s]');
ylabel('Error for state 1');
title('State estimation errors');
subplot(2,1,2);
plot(timeVector,eStates(:,2),...               % Error for the second state
    timeVector,sqrt(PCorrected(:,2,2)),'r', ...  % 1-sigma upper-bound
    timeVector,-sqrt(PCorrected(:,2,2)),'r');    % 1-sigma lower-bound
xlabel('Time [s]');
ylabel('Error for state 2');
legend('State estimate','1-sigma uncertainty bound',...
    'Location','Best');

センサー モデル (MeasurementFcn) なので、t=2.15 秒での状態 1 の誤差範囲は 0 に近づきます。この形式は です。t=2.15 秒では、実際の状態および推定された状態は 0 に近くなっています。これは、絶対項での測定値誤差も 0 に近いことを示しています。これは、フィルターの状態推定誤差の共分散に反映されます。

何パーセントの点が 1 シグマの不確実性範囲を超えているかを計算します。

distanceFromBound1 = abs(eStates(:,1))-sqrt(PCorrected(:,1,1));
percentageExceeded1 = nnz(distanceFromBound1>0) / numel(eStates(:,1));
distanceFromBound2 = abs(eStates(:,2))-sqrt(PCorrected(:,2,2));
percentageExceeded2 = nnz(distanceFromBound2>0) / numel(eStates(:,2));
[percentageExceeded1 percentageExceeded2]
ans =

    0.1386         0

最初の状態推定の誤差は、そのタイム ステップにおける の不確実性範囲である約 14% を超えています。1 シグマの不確実性範囲を超えている誤差の 30% 未満が、適切な推定であることを示します。この条件は両方の状態で満たされます。2 番目の状態に対する 0% という割合は、フィルターが保守的であることを意味しています。ほとんどの場合、組み合わせたプロセス ノイズと測定ノイズが高すぎます。これらのパラメーターを調整することで、パフォーマンスの向上を実現できる可能性があります。

状態推定誤差の平均の自己相関を計算します。また、自己相関をプロットします。

mean(eStates)
[xeStates1,xeStatesLags1] = xcorr(eStates(:,1),'coeff'); % 'coeff': normalize by the value at zero lag
[xeStates2,xeStatesLags2] = xcorr(eStates(:,2),'coeff'); % 'coeff'
% Only plot non-negative lags
idx = xeStatesLags1>=0;
figure();
subplot(2,1,1);
plot(xeStatesLags1(idx),xeStates1(idx));
xlabel('Lags');
ylabel('For state 1');
title('Normalized auto-correlation of state estimation error');
subplot(2,1,2);
plot(xeStatesLags2(idx),xeStates2(idx));
xlabel('Lags');
ylabel('For state 2');
ans =

   -0.0103    0.0200

誤差の平均値は、状態の値と比べて小さくなっています。状態推定誤差の自己相関は、小さなラグ値に対してランダムでない変化がほとんどないことを示しますが、これらは正規化されたピーク値 1 よりもはるかに小さいものです。推定された状態は正確であるという事実と組み合わせることで、残差に関するこの動作は満足できる結果であると考えることができます。

まとめ

この例では、非線形システムの状態推定値に対するアンセンテッド カルマン フィルターを作成および使用する手順を示しました。ノイズを含む測定値からファン・デル・ポール振動子の状態を推定しました。また、推定のパフォーマンスを検証しました。