はじめに
この記事ではカルマンフィルタを用いた外乱オブザーバーの設計について説明します。外乱オブザーバーは、システムにおける未知の外乱を推定し、制御性能を向上させるために使用されます。カルマンフィルタは、主に線形システムの状態推定に用いられるアルゴリズムであり、これを用いて外乱オブザーバーの設計に適用することができます。
カルマンフィルタの基本的な知識は使い方は前提とします。
外乱オブザーバー
以下の手順でカルマンフィルタを用いた外乱オブザーバーを設計できます。
- システムモデルの定義
- 外乱を含んだ拡大システムモデルの定義
- カルマンフィルタの設計
- アルゴリズムの実装
システムモデルの定義
システムの時間発展が状態空間モデルで定義できると仮定します。これは対象の設計ないし、何らかの手法で同定されているものとします。
$$ \begin{align} x(k+1) &= Ax(k) + Bu(k) + B _ d d(k) + w(k) \\ y(k) &= Cx(k) + v(k) \end{align} $$
ここで、
- $x(k)$ は状態ベクトル
- $y(k)$ は出力ベクトル
- $u(k)$ は入力ベクトル
- $d(k)$ は外乱ベクトル
- $A, B, C$ はシステム行列
- $w(k)$ はシステムノイズ
- $v(k)$ は観測ノイズ
- $k$ は時刻のインデックス
とします。通常の状態空間モデルと違うのは $B _ d d(k)$ という外乱が含まれている点です。ここでは入力ベクトル $u(k)$ と同じ $B _ d = B$ を想定してみましょう。すると$B(u(k) + d(k))$ となっており、入力が外から揺らされている状態を考えていることになります。例えば、倒立振子などに対して、外力(人が押す、強い空気抵抗を受ける)などが想定されています。
外乱を含んだ拡大システムモデルの定義
次に、システムを拡張して、外乱を状態ベクトルに含めます。新しい状態ベクトル x_e(k) を次のように定義します。 $$ x_e(k) = \begin{bmatrix} x(k) \\ d(k) \end{bmatrix} $$ 拡張状態方程式および出力方程式は以下のようになります。このように外乱を観測のできない状態変数の一部であると考えることで、新たなシステムモデル
$$ \begin{align} x_e(k+1) &= A_e x_e(k) + B_e u(k) + w_e(k) \\ y(k) &= C_e x_e(k) + v(k) \end {align} $$
ここで $A_e, B_e, C_e$ は拡張システム行列、$w_e(k)$ は拡張システムノイズで、外乱に対する成分も含んだものです。下記にシステム行列について関係性を書いておきます。
$$ \begin {align} A_e &= \begin{bmatrix} A & B _ d \\ 0 & I \end{bmatrix} \\ B_e &= \begin{bmatrix} B \\ 0 \end{bmatrix} \\ C_e &= \begin{bmatrix} C & 0 \end{bmatrix} \end{align} $$
カルマンフィルタの設計
拡大システムに対してカルマンフィルタを設計し、状態ベクトル x_e(k) を推定します。これは通常のカルマンフィルタ同様の手順になりますが、状態の初期値 $x _ e (0)$、共分散行列の初期値 $P(0)$、システムノイズ$Q$ と観測ノイズ $R$ の設定などは、外乱の成分も含まれていることに注意が必要です。ここはシステムの時間発展に対する不確かさや、観測に対する不確かさなどを考慮してドメイン知識で決めるか、センサーの静的な観測値のばらつきから推定値を代入するなどが考えられます。
アルゴリズムの実装
こちらは拡大状態空間モデルに対して通常通りにカルマンフィルタを適用することになります。状態の時間発展を下記の式で予測します。
$$ \hat{x _ e}{(k)} = A _ e \hat{x _ e}{(k-1)} + B _ e u(k-1) $$
これは状態の時間発展をモデル化したシステム方程式の通りに進めているだけです。次に共分散行列を予測します。
$$ P {(k)} = A _ e P{(k-1)} A _ e ^ T + Q $$
上記までを予測ステップといいます。次にフィルタリングステップで下記の計算を行います。まずはじめにカルマンゲインの計算です。
$$ K{(k)} = P{(k)} C _ e ^ T (C _ e P {(k)} C _ e ^ T + R) ^ {-1} $$
こちらのカルマンゲインを用いて状態の時間発展を、システムモデルによる単純な更新から修正します。この時点では新たな観測値 $y (k)$ が得られているものとします。時間発展をシステムモデルどおりに計算した場合とデータから逆算した場合の良い塩梅を取るのがカルマンゲインということになります。
$$ \hat {x} _ e (k) \leftarrow \hat{x} _ e (k) + K(k) (y(k) - C _ e \hat{x} _ e (k)) $$
今回の観測値を利用して共分散行列の推定値も更新されます。
$$ P(k) \leftarrow (I - K(k) C _ e) P(k) $$
数値実験
適当なトイデータでカルマンフィルタによる外乱オブザーバーを試してみます。状態変数は二次元で、入力と出力および外乱は一次元という設定にします。すなわち、観測できるのは2つの状態のうち1つだけのときに、入力データが外乱によって揺らされているようなケースで、正しく状態を推定できるのかという問題と、入力を汚している外乱を推定したいという問題を同時に解くということになります。
対象のモデル
状態空間モデルのパラメータは下記とします。
$$ A = \begin{bmatrix} 0.8 & 0.1 \\ -0.2 & 0.9 \end{bmatrix},\ B = \begin{bmatrix} 0.5 \\ 0.3 \end{bmatrix},\ B_d = \begin{bmatrix} 0.6 \\ -0.4 \end{bmatrix},\ C = \begin{bmatrix} 1 & 0 \\ 0 & 0 \end{bmatrix} $$
したがって拡大状態空間モデルは以下のように表現できます。
$$ A_e = \begin{bmatrix} A & B_d \\ \begin{matrix} 0 \end{matrix} & I \end{bmatrix},\ B_e = \begin{bmatrix} B \\ 0 \end{bmatrix},\ C_e = \begin{bmatrix} C & 0 \end{bmatrix} $$
という形式ですので、具体的な行列は下記のような数字が埋まるはずです。
$$ A_e = \begin{bmatrix} 0.8 & 0.1 & 0.6 \\ -0.2 & 0.9 & -0.4 \\ 0 & 0 & 1 \end{bmatrix},\ B_e = \begin{bmatrix} 0.5 \\ 0.3 \\ 0 \end{bmatrix},\ C_e = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} $$
コード
実装はMATLABですが行列の計算を行っているのでNumpy等で試してみることも可能です。
% Parameters A = [0.8, 0.1; -0.2, 0.9]; B = [0.5; 0.3]; Bd = [0.6; -0.4]; C = diag([1, 0]); % Number of iterations N = 100; % Initialize system state, control input, and disturbance x = zeros(2, N); u = 1; d = 0.1 * sin([1:N] * 0.2)'; % Initialize Kalman filter matrices A_e = [A, Bd; zeros(1, 2),eye(1)]; B_e = [B; zeros(1, 1)]; C_e = [C, zeros(2, 1)]; Q = 1 * eye(3); R = 0.001; % Kalman filter implementation x_e = zeros(3, N); P = Q; for k = 2:N-1 % State update x(:, k) = A * x(:, k-1) + B * u + Bd * d(k-1); % Measurement update y = C * x(:, k); % Kalman filter prediction [x_e(:,k), P] = kalman_pred(A_e,B_e,Q,P,x_e(:,k-1),u); % Kalman filter update [x_e(:,k), P, K] = kalman_filt(C_e, P, R, x_e(:,k), y); end % Plot results figure; subplot(3, 1, 1); plot(1:N, x(1, :), 'b', 1:N, x_e(1, :), 'r'); xlabel('Time Step'); ylabel('State x1'); legend('True state x1', 'Estimated state x1'); title('State Estimation'); subplot(3, 1, 2); plot(1:N, x(2, :), 'b', 1:N, x_e(2, :), 'r'); xlabel('Time Step'); ylabel('State x2'); legend('True state x2', 'Estimated state x2'); title('State Estimation'); subplot(3, 1, 3); plot(1:N, d, 'b', 1:N, x_e(3, :), 'r'); xlabel('Time Step'); ylabel('Disturbance d'); legend('True disturbance', 'Estimated disturbance'); title('Disturbance Estimation'); function [x, P] = kalman_pred(A,B,Q,P,x,u) x = A * x + B * u; P = A * P * A' + Q; end function [x, P, K] = kalman_filt(C, P, R, x, y) K = P * C' / (C * P * C' + R); x = x + K * (y - C * x); P = (eye(size(P,1)) - K * C) * P; end
観測できている状態1の方は実質、観測値との照合を上手く行うだけなので良く推定できています。一方で未観測の状態2も、カルマンフィルタの状態推定がしっかり働いて、推定ができていそうです。一方で外乱は少し推定が遅れているように見えますが計上は概ね捉えられていると言えるでしょう。
ただし実際の応用ではカルマンフィルタの設計を適切に行う必要があります。例えば、今回のケースはホワイトノイズなどのノイズ項を含んでいないので、カルマンフィルタの設計において観測誤差のパラメータを小さくできました。実際にノイズがあると、観測が信用出来ないということを反映すべく観測誤差を大きく設定せざるをえなくなり、状態推定は遅れを持つことになります。制御の応用上では、外乱を推測できたことによって本来は状態を揺らがせたくないというときに外乱の推定値をもとに次の入力値を修正することで適切な制御を行うことに繋げられます。
今回は以上になります。