。

カルマンカルマン经理

ライブスクリプトを開く

このケーススタディでは,カルマンフィルター設計およびシミュレーションについて説明します。ここでは,定常状態カルマンフィルターおよび時変カルマンフィルターの両方について考えます。

プラントダイナミクス

入力 $u ［ n ］$ に加法性のガウスノイズ $w ［ n ］$ をもつ离散プラントをますます。

$\begin{array}{l} x ［ n + 1 ］＝一个 x ［ n ］ + B （ u ［ n ］ + w ［ n ］） \\ y ［ n ］＝ C x ［ n ］． \end{array}$

さらに, $y_{v} ［ n ］$ をを力 $y ［ n ］$ のノイズを含む测定値ととし， $v ［ n ］$ は測定ノイズを示すものとします。

$y_{v} ［ n ］＝ y ［ n ］ + v ［ n ］．$

以下の行列は,プラントのダイナミクスを表します。

A = [1.1269 -0.4940 0.1129;1.0000 0 0;1.0000 0 0);B = (-0.3832;0.5919;0.5191);C = [1 0 0];

離散カルマンフィルター処理

この问题の定常状态のカルマンフィルター方程式，次次とおりです。

测定値の更新：

$x_{}^{ˆ} ［ n | n ］＝ x_{}^{ˆ} ［ n | n - 1 ］ + 米（ y_{v} ［ n ］ - C x_{}^{ˆ} ［ n | n - 1 ］）$

時刻の更新:

$x_{}^{ˆ} ［ n + 1 | n ］＝一个 x_{}^{ˆ} ［ n | n ］ + B u ［ n ］$

この方程式では，次のようになり。

$x_{}^{ˆ} ［ n | n - 1 ］$ は, $y_{v} ［ n - 1 ］$ までの過去の測定値からの $x ［ n ］$ のの値。
$x_{}^{ˆ} ［ n | n ］$ は,最後の測定値 $y_{v} ［ n ］$ に基づき更新された推定値。

現在の推定値を $x_{}^{ˆ} ［ n | n ］$ とすると,時刻の更新によって,次のサンプルn+ 1(1ステップ先の予測子)での状態値が予測されます。測定値の更新により,新しい測定値 $y_{v} ［ n + 1 ］$ 修正项ます调整されががささはは修されははささはは，イノベーションの关联，つまり $y ［ n + 1 ］$ の測定値と予測値間の誤差の関数です。この誤差は次のように与えられます。

$y_{v} ［ n + 1 ］ - C x_{}^{ˆ} ［ n + 1 | n ］$

イノベーションゲインMはノイズ共分散に対する推定誤差の定常状態共分散を最小にするように選択します。

$E （ w ［ n ］ w {［ n ］}^{T} ）＝问 E （ v ［ n ］ v {［ n ］}^{T} ）＝ R N ＝ E （ w ［ n ］ v {［ n ］}^{T} ）＝ 0$

時刻および測定値更新方程式を組み合わせて,1つの状態空間モデル(カルマンフィルター)を生成できます。

$\begin{array}{l} x_{}^{ˆ} ［ n + 1 | n ］＝一个（我 - 米 C ） x_{}^{ˆ} ［ n | n - 1 ］ + ［ \begin{array}{cccccccccccccccccccc} B & 一个米 \end{array} ］［ \begin{array}{cccccccccccccccccccc} u ［ n ］ \\ y_{v} ［ n ］ \end{array} ］ \\ y_{}^{ˆ} ［ n | n ］＝ C （我 - 米 C ） x_{}^{ˆ} ［ n | n - 1 ］ + C 米 y_{v} ［ n ］． \end{array}$

このフィルターにより, $y_{n}$ の最適推定値 $y_{}^{ˆ} ［ n | n ］$ が生成され。フィルターの状态は $x_{}^{ˆ} ［ n | n - 1 ］$ であることに注意してください。

定常状态の设计

関数卡尔曼を使用して,上記の定常状態カルマンフィルターを設計できます。まず,次のプロセスノイズを含むプラントモデルを設定します。

$\begin{array}{cccccccccccccccccccc} x ［ n + 1 ］＝一个 x ［ n ］ + B u ［ n ］ + B w ［ n ］ \\ y ［ n ］＝ C x ［ n ］ \end{array}$

ここで,最初の式は状態方程式,2番目の式は測定方程式です。

次のコマンドにより,このプラントモデルを指定します。サンプル時間を指定せずにモデルを離散とマークするために,サンプル時間を1に設定します。

(A，[B B]，C,0，-1，)“inputname”，{“u”' w '}，'outputname'，“y”)；

问＝R= 1と仮定して,離散カルマンフィルターを設計します。

Q = 1;R = 1;[kalmf L P M] =卡尔曼(植物,Q, R);

このコマンドは,フィルターの状態空間モデルkalmf.とイノベーションゲイン米ををます。

米

M =3×10.3798 0.0817 -0.2570

kalmf.の入力はuと $y_{v}$ です。出力はプラント出力と状態推定値 $y_{e} ＝ y_{}^{ˆ} ［ n | n ］$ および $x_{}^{ˆ} ［ n | n ］$ です。

ここでは出力推定値 $y_{e}$ に注目しているため，kalmf.の最初のの力量をしてて残りは破弃ししますます。

: kalmf = kalmf (1);

フィルターがどのように機能するかを確認するには,何らかの入力データとランダムノイズを生成し,フィルターを適用した応答 $y_{e}$ を真の応答yと比较します。それぞれの応答は，别々别々生成するも，同时に生成することできます。それぞれそれぞれの応答をを别々にシミュレートににははははははにははlsimををプラントだけで使，次次にプラントとフィルターを绪にしてて使てますますますを同にシミュレーションシミュレーションもうもうもう同についてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについてについて

次のブロック線図は,真の出力とフィルターを適用した出力を生成する方法を示します。

このブロック線図の状態空間モデルは,関数平行线と関数回馈を使って構成できます。まず,u、w、vを入力とし,yと $y_{v}$ (測定値)を出力とする完全なプラントモデルを作成します。

a = a;b = [b b 0 * b];c = [c; c];d = [0 0 0; 0 0 1];p = ss（a，b，c，d，-1，“inputname”，{“u”' w '“v”}，'outputname'，{“y”'yv'})；

次に平行线をを使し，次次の図の并列接続をををしし

SYS =并联（P，Kalmf，1,1，[]，[]）;

最後に,正のフィードバックによりプラント出力 $y_{v}$ をフィルター入力 $y_{v}$ に接続してセンサーループを閉じます。

SimModel =反馈(sys, 1、4、2、1);输入＃4附近的％闭环和输出＃2SimModel = SimModel([1 3]，[1 2 3]);%从I/O列表中删除yv

結果として得られるシミュレーションモデルは,入力としてw、v、uをを，出力としてyと $y_{e}$ をもちます。InputNameプロパティおよびOutputNameプロパティを表示して確認します。

SimModel。InputName

ans =3 x1细胞{' w '} {' v '} {' u '}

SimModel。OutputName

ans =2 x1细胞{' y '} {' y_e '}

これで，フィルターの驾驶をシミュレーションする入ました。正弦波入力u,プロセスノイズベクトルw,測定ノイズベクトルvを生成します。

t =[0:100]”;u =罪(t / 5);n =长度(t);rng默认的w = sqrt (Q) * randn (n, 1);v = sqrt (R) * randn (n, 1);

応答のシミュレーションを実行します。

(, x) = lsim (SimModel [w, v, u]);y = (: 1);％真实响应你们= (:,2);％过滤响应Yv = y + v;%响应测量

真の応答とフィルターを适た応答をグラフで比较します。

子图（211），绘图（t，y，“——”, t,你们,“- - -”),包含(“不。样品的), ylabel (“输出”） 标题（“卡尔曼滤波器响应”)次要情节(212)、图(t y-yv“-”。t y-ye“- - -”),包含(“不。样品的), ylabel (“错误”）

最初最初のプロット，真の応答y（绕线）とフィルターを适适适た力 $y_{e}$ （铁线）をを示しています第第のプロット，测定误差（锁锁と点）と推定误差（铁路）を误差ししてます。このプロットは,ノイズレベルが著しく減少したことを示しています。これは,共分散誤差を計算して確認します。フィルター処理を行う前の誤差共分散(測定誤差)は,次のとおりです。

MeasErr = y-yv;MeasErrCov = (MeasErr。* MeasErr)之和/长度(MeasErr)

MeasErrCov = 0.9992

一方,フィルター処理を行った後の誤差共分散(推定誤差)は,次のように減少します。

EstErr = y-ye;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr)

EstErrCov = 0.4944

時変カルマンフィルター

時変カルマンフィルターは,時変システム,または非定常ノイズ共分散を含むLTIシステム用に定常状態フィルターを一般化したものです。

次次プラントの状态および测定式式について考えます。

$\begin{array}{rcl} x ［ n + 1 ］ & ＝ & 一个 x ［ n ］ + B u ［ n ］ + G w ［ n ］ \\ y_{v} ［ n ］ & ＝ & C x ［ n ］ + v ［ n ］． \end{array}$

時変カルマンフィルターは,次の再帰計算で与えられます。

测定値の更新：

$\begin{array}{rcl} x_{}^{ˆ} ［ n | n ］ & ＝ & x_{}^{ˆ} ［ n | n - 1 ］ + 米［ n ］（ y_{v} ［ n ］ - C x_{}^{ˆ} ［ n | n - 1 ］） \\ 米［ n ］ & ＝ & P ［ n | n - 1 ］ C^{T} （ R ［ n ］ + C P ［ n | n - 1 ］ C^{T} ）^{- 1} \\ P ［ n | n ］ & ＝ & （我 - 米［ n ］ C ） P ［ n | n - 1 ］． \end{array}$

時刻の更新:

$\begin{array}{rcl} x_{}^{ˆ} ［ n + 1 | n ］ & ＝ & 一个 x_{}^{ˆ} ［ n | n ］ + B u ［ n ］ \\ P ［ n + 1 | n ］ & ＝ & 一个 P ［ n | n ］ {一个}^{T} + G 问［ n ］ G^{T} ． \end{array}$

ここで, $x_{}^{ˆ} ［ n | n - 1 ］$ および $x_{}^{ˆ} ［ n | n ］$ は，前述のとおりです。さらに，以下が成立し。

$\begin{array}{l} 问［ n ］＝ E （ w ［ n ］ w {［ n ］}^{T} ） \\ R ［ n ］＝ E （ v ［ n ］ v {［ n ］}^{T} ） \\ P ［ n | n ］＝ E （｛ x ［ n ］ - x ［ n | n ］｝ {｛ x ［ n ］ - x ［ n | n ］｝}^{T} ） \\ P ［ n | n - 1 ］＝ E （｛ x ［ n ］ - x ［ n | n - 1 ］｝ {｛ x ［ n ］ - x ［ n | n - 1 ］｝}^{T} ）． \end{array}$

簡略化するために,状態空間行列の時間依存性を示す添字を省略しました。

初期条件 $x ［ 1 | 0 ］$ および $P ［ 1 | 0 ］$ が与えられると,これらの方程式を反復してフィルター処理を実行できます。時間サンプルごとに状態推定値 $x ［ n | ．］$ と誤差共分散行列 $P ［ n | ．］$ の両方を更新しなければなりません。

時変設計

これらのフィルターの再帰処理を実装するには,まず,ノイズの加わった出力観測量を生成します。前に生成したプロセスノイズwと測定ノイズvを使用します。

sys = ss（a，b，c，0，-1）;Y = LSIM（SYS，U + W）;Yv = y + v;

次の初期条件を仮定します。

$\begin{array}{cccccccccccccccccccc} x ［ 1 | 0 ］＝ 0 ， & P ［ 1 | 0 ］ \end{array} ＝ B 问 B^{T}$

为ループをもつ时代フィルターフィルター装配します。

P = B * * B”;%初始误差协方差x = 0 (3,1);州的初始条件你们= 0(长度(t), 1);ycov = 0(长度(t), 1);为i = 1:长度(t)％测量更新mn = p * c'/（c * p * c'+ r）;x = x + mn *（yv（i）-c * x）;％x [n | n]P =(眼(3)mn * C) * P;％p [n | n]你们(i) = C * x;errcov (i) = C * P * C ';%的时间更新x = A*x + B*u(i);% x [n + 1 | n]P = a *P* a ' + b * q * b ';% P [n + 1 | n]结束

真の出力と推定出力をグラフ上で比較します。

子图（211），绘图（t，y，“——”, t,你们,“- - -”） 标题（“时变卡尔曼滤波器响应”)包含(“不。样品的), ylabel (“输出”)次要情节(212)、图(t y-yv“-”。t y-ye“- - -”)包含(“不。样品的), ylabel (“输出”）

最初最初のプロット，真の応答y(破線)とフィルターを適用した応答 $y_{e}$ （铁线）をを示しています第第のプロット，测定误差（锁锁と点）と推定误差（铁路）を误差ししてます。

时代フィルターは，サンプルごとに推定误差 $y - y_{e}$ の共分散errcovも推定します。それをプロットして,フィルターが(定常入力ノイズで期待されるとおりに)定常状態に達したかどうかを確認します。

次要情节(211)情节(t, errcov) ylabel (“柯伐合金错误”）

この出で実际およそしし，时尚フィルター定常状态と以降性能をし状态と性能を维持定常状态同じ性能を定常ますし同じ性能をフィルターは定常状态と同じ性能フィルターは定常状态し同じサンプルたは定常状态と同じ性能フィルターは定常定常と同じ性能フィルターはし定常状态同じ性能共フィルターは定常およそと同じ共フィルターフィルターは定常およそと同じ共共フィルターは定常およそでき同じ共共ではおよそますと同じ共共ではプロットおよそしつのし共共でプロットおよそますしサンプル共共共およそおよそますし以降共就会

実験データから得られた推定誤差の共分散と比較します。

Esterr = Y  -  YE;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr)

EstErrCov = 0.4934

この値は，理想値errcovより小さく,定常状態の設計で得られる値に近くなります。

最後に,最終値 $米［ n ］$ とイノベーションゲイン行列の定常値米が一般することに注意てて。

锰

Mn =3×10.3798 0.0817 -0.2570

米

M =3×10.3798 0.0817 -0.2570

参考文献

[1] Grimble M.J,鲁棒工业控制:多项式系统的优化设计方法，Prentice Hall，1994，p。261和pp。443-456。

カルマンカルマン经理

プラントダイナミクス

離散カルマンフィルター処理

定常状态の设计

時変カルマンフィルター

時変設計

参考文献

関連するトピック

控制系统工具箱ドキュメンテーション

サポート

学习如何自动调整PID控制器增益