ふみきりのぼうけん

電子工作、プログラミング、旅行、書籍紹介などの雑多なブログです。

DCモータのパラメータモニタリングと故障検知

はじめに

この記事は制御工学 Advent Calendar 2018 - Qiita の21日目の記事です。

制御工学の知識はおおよそ追従性能の改善や外乱推定など、制御性能の改善に注目されがちですが、今回はその知識をパラメータモニタリングや故障検知へ使う方法をこちらのテキストを参考にしながら学びたいと思います。厳密に言えば制御工学ではないかもしれませんが、部分的に使っているので許してください笑

今回はこのテキストの6章 pp.195-202に倣います。
扱う対象はシンプルなブラシ付きDCモータです。テキスト内ではDCモータの故障検知について実験で結果が示されています。
今回の記事では、だれでもシミュレーションによって実験を確認できるよう、実験を元にサンプルコードを作成しましたのでそれを使ってシミュレーションしながら内容を確認していきます。
英語のテキストや論文を読み適宜和訳しながら説明を書いているので、ちょっと訳が変だったりするところがあると思います。そのため、気になったところは訳と英単語を両方書いてあります。

この記事で学べること

パラメータモニタリングとparity equationsを組み合わせた故障検知の方法

制御対象のモデリング

制御対象であるDCモータのモデリングを行います。ブラシ付きの一般的なDCモータを想定しており、非線形要素も含めた運動方程式は下記のようになります。

 L_A\dot{I}_A = -R_AI_A(t)-\Psi\omega(t)-K_B|\omega(t)|I_A(t)+u_a(t)
 J\dot{\omega}=\Psi I_A(t)-M_{F1}\omega(t)-M_{F0}{\rm sign}(\omega(t))-M_L(t)

ここで、それぞれの記号の意味は下記の表にまとめてあります。

記号の名前 記号と値
armature voltage  u_a
armature current  I_A
speed   \omega
armature resistance  R_A=1.52 \ \Omega
armature inductance  L_A=6.82 \times 10^{-3}\  \Omega\  {\rm s}
magnetic flux  \Psi=0.33 \ {\rm \ V \ s}
voltage drop factor  K_B=2.21  \ \times 10^{-3} {\rm \ V \ s \ / \ A}
inertia constant  J=1.92  \times 10^{-3} \ {\rm kg  \ m^2}
viscous friction  M_{F1}=0.36 \times 10^{-3} \ {\rm Nm \ s}
dry friction  R_A=0.11 \ {\rm \  Nm}

 K_B\|\omega(t)\|I_A(t)はモータの逆起電力を補償するために追加されているようです。定格は550Wで、2500rpmで回転させます(と書いてあります)。

拡張カルマンフィルタは非線形なシステムに対応していますが、できるだけ簡単にするため、制御対象の非線形要素(誘導起電力や摩擦)をシステムへの入力の一部とすることで非線形要素を追い出して線形な状態方程式の形へ変形します。
具体的には入力変数を下記のように非線形要素を入力の一部とした形で定義します。
{\displaystyle

\begin{align}
	\dot{x}  =
\left(
    \begin{array}{c}
    \dot{I}_A \\
    \dot{\omega}
    \end{array}
  \right)
  =
  \left(
    \begin{array}{cc}
    -\frac{R_A}{L_A} & -\frac{\Psi}{L_A} \\
    \frac{\Psi}{J} & -\frac{M_{F1}}{J} 
    \end{array}
  \right)
  \left(
    \begin{array}{c}
    {I}_A \\
    {\omega}
    \end{array}
  \right) +
  \left(
    \begin{array}{cc}
    \frac{1}{L_A} & 0 \\
    0 & -\frac{1}{J} 
    \end{array}
  \right)
  \left(
    \begin{array}{c}
    {U}_A \\
    {M}_L
    \end{array}
  \right)
\end{align}
}
 U_A = u_a-K_B|\omega(t)|I_A
 M_L = M_{F0}{\rm sign}\omega(t)

これで線形化することができました。

故障検知と分離(FDI: fault detection and isolation)について

一般的なFDIの方法は主に2つの部分から構成されます。

1.症状の生成部分(symptom generation part)

症状は状態方程式から推定されたパラメータと残差(residuals)から生成され、これを用いて故障を検知します。

2.診断部分(diagnostic part)

症状の生成部分から生成された症状から、故障原因との関係を構築します。

故障検知と分離のために、重要な症状を発見できるとともに、それがノイズや外乱に対してロバストである必要があります。

1.症状の生成部分(symptom generation part)

こちらのテキストでは大きく分けて2つの手法を用いてを用いることで故障を検知しています。

Ⅰ パラメータ推定
Ⅱ 残差(residuals)

Ⅰ パラメータ推定とその問題点

故障検知についてはMatlabで拡張カルマンフィルタを使ったパラメータ推定が紹介されています。
jp.mathworks.com
テキストではDSFI(Discrete square root filtering in information form)という手法を用いて各パラメータの推定を行っています。ただ、実験結果は載っているのですが、この手法がこのテキストに詳しく書かれていなく、調べたところちょっと習得するのに時間がかかりそうだったので、今回の記事では代わりに拡張カルマンフィルタを用いた手法でR_Aのみ推定を行い、シミュレーションを行います。
パラメータ推定における利点は物理パラメータの変動があってもオンラインで推定でき、制御器を変更するなどの対策が取れる点です。

カルマンフィルタの設計方法は下記のサイトを参考にしました。基本的なカルマンフィルタについてほぼ網羅されています。
myenigma.hatenablog.com
qiita.com
qiita.com

1. R_A故障(\Delta R_A:+50\%)でのパラメータ推定シミュレーション

下図にパラメータ推定を行った結果を示します。R_Aの変動を拡張カルマンフィルタにて適切に推定することができています。
シミュレーション条件
・入力電圧u_aに振幅15Vの矩形波入力
U_Aにp-p.3Vのランダム雑音
・0~4秒間はパラメータ変動なし、4~6秒間にパラメータ変動あり

f:id:fumikirinobouken:20181211174757p:plain
矩形波入力時の各時間応答(\Delta R_A: +50%)
f:id:fumikirinobouken:20181211174610p:plain
R_Aの真値と推定値

2.電圧測定センサーゲインU_Aの故障(\Delta U_A:+10\%)でのパラメータ推定シミュレーション

先程のシミュレーションでR_Aの値を正確に推定することができました。しかしこれはシステムからの出力が正確に測定できるという環境下のことであり、故障検知に用いる場合、センサーの故障によるセンサーゲインの変動があると物理パラメータの推定値に影響が出てしまい、物理パラメータは正しいのにその推定値が変化しているように見えてしまうという問題があります。
そこで、電圧測定センサーU_Aのゲインに10%の誤差があるという条件でパラメータ推定シミュレーションを行います。
下記の図を見ると、物理パラメータが変動していないにもかかわらず、推定したパラメータは元のパラメータとは異なる値を示しています。
すなわち、故障検知にパラメータ推定を用いる場合、これに加えてさらに複数の判断基準を元に判断することが必要となります。
この問題を解決すべく、テキストではパラメータ推定だけでなく、parity equationsを用いた故障検知が紹介されています。

シミュレーション条件
・入力電圧u_aに振幅15の矩形波入力
U_Aにp-p.3Vのランダム雑音
・0~4秒間はパラメータ変動なし、4~6秒間にパラメータ変動あり

f:id:fumikirinobouken:20181211175124p:plain
矩形波入力時の各時間応答(\Delta U_A: +10%)
f:id:fumikirinobouken:20181211175515p:plain
R_Aの真値と推定値

Ⅱ parity equations を用いた残差(residuals)による故障検知

parity equationsは故障検知のための残差(residuals)を生成する一つの方法です。この方法は推定された状態や測定された状態から求められた偏差に基づいて故障検知が行われます。
偏差はプラントの入力と出力から運動方程式などを用いることで常に計算され、実プラントとノミナルモデルが一致するなら0となるように選ばれています。
このparity equationsの選び方は様々な方法があり、伝達関数モデル、連続・離散状態方程式モデル等、目的や制御対象に合わせて選ばれるようです。

今回は、DCモータの運動方程式を用い、偏差r_e(s)を下記にように表します。
 r_{e1}(t) =0= L_A\dot{I}_A +R_AI_A(t)+\Psi\omega(t)-U_A(t)
 r_{e2}(t) =0= J\dot{\omega}-\Psi I_A(t)+M_{F1}\omega(t)+M_L(t)

これをラプラス変換すると、
 \begin{align}
r_{e1}(s) &= 0 = (L_As+R_A)I_A(s)+\Psi\omega(s)-U_A(s) \\
r_{e2}(s) &= 0 = -\Psi I_A(t)+(Js+M_{F1})\omega(t)+M_L(t) 
\end{align}
となります。

これをさらに変換して
 r_{e} =
\begin{align}
0 = \left(
\begin{array}{c}
    1 \\
    0 
    \end{array}
  \right)U_A(s)
   -\left(
\begin{array}{c}
    R_A+L_As \\
    0-\Psi
    \end{array}
  \right)I_A(s)
  -\left(
\begin{array}{c}
    \Psi \\
    M_{F1}+Js
    \end{array}
  \right)\omega(s)
  +\left(
\begin{array}{c}
    0 \\
    1 
    \end{array}
  \right)M_L(s)
\end{align}

ここで、残差生成行列 W(s)を用いると残差 r(s)は下記のようになります。
\begin{align}
r(s) =W(s)r_e(s)= W(s)\left(
\left(
\begin{array}{c}
    1 \\
    0 
    \end{array}
  \right)U_A(s)
   -\left(
\begin{array}{c}
    R_A+L_As \\
    -\Psi
    \end{array}
  \right)I_A(s)
  -\left(
\begin{array}{c}
    \Psi \\
    M_{F1}+Js
    \end{array}
  \right)\omega(s)
  +\left(
\begin{array}{c}
    0 \\
    1 
    \end{array}
  \right)M_L(s)
  \right)
\end{align}

この残差生成行列 W(s)をセンサーの故障を検知できるように設計していきます。 W(s)の詳しい設計方法はちょっとだけ数式ずらーになるので付録1にまとめました。

 W(s)を用いることでDCモータの残差は下記のように4つの式が選ばれます。
 \begin{align}
	r_1(t) &= L_A\dot{I}_A+R_AI_A(t)+\Psi\omega(t)-U_A\\
	r_2(t) &= J\dot{\omega}-\Psi I_A(t)+M_{F1}\omega(t)+M_L(t) \\
	r_3(t) &= JL_A\ddot{I}_A(t)+(L_AM_{F1}+JR_A)\dot{I}_A(t)+(\Psi^2+R_AM_{F1})I_A-J\dot{U}(t)_A-M_{F1}U_A(t)-\Psi M_L(t) \\
	r_4(t) &= JL_A\ddot{\omega(t)}+(L_AM_{F1}+JR_A)\dot{\omega(t)}+(\Psi^2+R_AM_{F1})\omega(t)-\Psi U_A(t)+L_A\dot{M}_L(t)+R_AM_L(t)
\end{align}


これらの残差は下記の条件に従って選ばれています。
 r_1(t):入力M_L(t)に独立
 r_2(t):入力U_A(t)に独立
 r_3(t):出力\omega(t)に独立
 r_4(t):出力I_A(t)に独立

すなわち、残差が4つ選ばれる理由は、この系においてセンサーによって取得される値が M_L U_A I_A \omegaの4つなので、それぞれに独立した残差の式が4つできるように残差生成行列 W(s)が決定されているからです。
残差 r(t)は偏差 r_e(t) W(t)が乗算されているだけなので、誤差がない限り、これら4つの式の出力は0になるように設計されています。
しかし、パラメータ変動やセンサーゲイン誤差が生じる場合、出力が0になるということが成り立たなくなります。

すなわち、それぞれの入力、出力から独立した残差の式を用いることで、応答の有無を調べることにより誤差があるセンサを見分けることができるのです。

具体例を示すと、\omega(t)を測定する速度センサにオフセット誤差が生じると、\omega(t)に独立な r_3(t)は変化せず、 r_1(t),  r_2(t),  r_4(t)のみ出力が0でなくなるわけです。
これにより、 r_3(t)のみ変化していない、ということは\omega(t)を測定する速度センサに誤差がある、と判断することができます。
この残差による検知とパラメータ推定を組み合わせることでパラメータ変動とセンサ誤差を区別して検知できるようになります。


2.診断部分(diagnostic part)

パラメータ推定と残差を用いて特徴を抽出したあとは、下記の表を用いて故障の原因を判定します。図中の記号は下記の意味を持ちます。
++:大きく正の値に偏差
+ : 正の値に偏差
0 : 偏差なし
ー :負の値に偏差
ーー:大きく負の値に偏差
± :値が激しく変動

https://www.amazon.co.jp/exec/obidos/ASIN/B071YWFKNM/hatena-blog-22/


この表とテキスト補足説明によると、物理パラメータR_AU_Aを取得するセンサーゲインに関して変動があったときの推定値と残差は下記の特徴を持ちます。

1. R_A故障(\Delta R_A:+50\%)

この故障によりパラメータ推定ではR_Aの変動が推定されます。また、残差が激しく変動するようになり、残差1で一定入力に対し小さなオフセットを生じます。

2.電圧測定センサーゲインU_Aの故障(\Delta U_A:+10\%)

この故障により、パラメータ推定では物理パラメータは正しくても電圧測定センサーゲインが推定モデルにモデリングされていないため、 R_A, \ L_A,\ \Psiの推定値が(不正確に)変化し、変動が大きくなります。
残差rを見ると残差1(3と4)がマイナス方向にオフセットを持ち、激しく変動するようになります。しかし、2はU_Aに対して独立であるため変動しないという特徴を持ちます。

この表はパラメータ推定の方法により変化するため、手法に合わせて安定性の解析と外乱応答を検証する必要がありますが、今回はとりあえずこの表とテキストの通りに検証を行っていきます。

3.シミュレーション結果

シミュレーションのためのサンプルコードを付録2に載せておきます。

1. R_A故障(\Delta R_A:+50\%)でのパラメータ推定シミュレーション

物理パラメータR_AU_Aを取得するセンサーゲインに関して変動があったときの推定値と残差を比較します。
シミュレーション条件
・入力電圧u_aに振幅15Vの矩形波入力
U_Aにp-p.3Vのランダム雑音
・0~4秒間はパラメータ変動なし、4~6秒間にパラメータ変動あり

まず、物理パラメータR_Aが1.5倍になったときの入出力、推定値、残差を下図に示します。
拡張カルマンフィルタによるR_Aの推定をうまく推定できています。
また、表の通り残差においてパラメータ変動後、r_1(t)r_3(t)r_4(t))において激しく変動するようになっています。

f:id:fumikirinobouken:20181211174610p:plain
R_Aの真値と推定値
f:id:fumikirinobouken:20181213170719p:plain
残差の応答(\Delta R_A: +50\%)

2.電圧測定センサーゲインU_Aの故障(\Delta U_A:+10\%)でのパラメータ推定シミュレーション

次に、電圧測定センサーゲインU_Aの故障によりゲインが1.1倍になったときの入出力、推定値、残差を下図に示します。
カルマンフィルタではR_Aにて推定誤差を生じており、推定値が激しく変動するようになっています。
こちらも表のとおり、残差においてパラメータ変動後、r_1(t)r_4(t)において+入力に対して負の定常値を示しています。(r_3(t)に関してはU_Aに乗算されている項が M_{F1}=0.36 \times 10^{-3} \ {\rm Nm \ s}であるため、影響が小さくなっていると考えられます。)
一方で、r_2(t)においてはパラメータ変動前後で応答に変化がありません。こちらも定義どおりですね。

f:id:fumikirinobouken:20181211175515p:plain
R_Aの真値と推定値
f:id:fumikirinobouken:20181213171017p:plain
残差の応答(\Delta U_A: +10\%)
このように、パラメータ推定だけでなく残差を用いることで、どこが、どんなふうに故障したかというのが数学的な根拠を持って調べることができます。テキストを読んでいるとこんな制御や方程式の使い方があるんだという驚きがたくさんありました。

4.まとめ

今回はあんまり馴染みのないモデルベースな故障検知についてまとめてみました。最初はおもしろそーだしお手頃そうじゃんとか思ってたんですが、やってみると知らないことが結構あって勉強にはなりましたが、まとめるのが大変でした。安全のための冗長性が重要になっている自動車とかにこういう仕組みが組み込まれてるんでしょうか。複雑なシステムほどこういう故障検知は難しくなる、けど必要になってくるんでしょうね…。



付録1 DCモータの残差の求め方

運動方程式
 L_A\dot{I}_A = -R_AI_A(t)-\Psi\omega(t)+U_A(t)
 J\dot{\omega}=\Psi I_A(t)-M_{F1}\omega(t)-M_L(t)
ラプラス変換すると、
 \begin{align}
0 = (L_As+R_A)I_A(s)+\Psi\omega(s)-U_A(s) \\
0 = -\Psi I_A(t)+(Js+M_{F1})\omega(t)+M_L(t) 
\end{align}
となります。これをさらに変換して
 
\begin{align}
0 = \left(
\begin{array}{c}
    1 \\
    0 
    \end{array}
  \right)U_A(s)
   -\left(
\begin{array}{c}
    R_A+L_As \\
    0-\Psi
    \end{array}
  \right)I_A(s)
  -\left(
\begin{array}{c}
    \Psi \\
    M_{F1}+Js
    \end{array}
  \right)\omega(s)
  +\left(
\begin{array}{c}
    0 \\
    1 
    \end{array}
  \right)M_L(s)
\end{align}
ここで、残差生成行列 W(s)と残差 r(s)の関係は
\begin{align}
r(s) = W(s)\left(
\left(
\begin{array}{c}
    1 \\
    0 
    \end{array}
  \right)U_A(s)
   -\left(
\begin{array}{c}
    R_A+L_As \\
    -\Psi
    \end{array}
  \right)I_A(s)
  -\left(
\begin{array}{c}
    \Psi \\
    M_{F1}+Js
    \end{array}
  \right)\omega(s)
  +\left(
\begin{array}{c}
    0 \\
    1 
    \end{array}
  \right)M_L(s)
  \right)
\end{align}
となります。ここでのセンサーから取得する入出力は M_L U_A I_A \omegaの4つなので、それぞれに独立した残差の式が4つできるように残差生成行列 W(s)を決定していきます。
 W(s)=\left(
\begin{array}{c}
    w_1(s) \\
    w_2(s) \\
    w_3(s) \\
    w_4(s) 
    \end{array}
  \right)
とすると、
 
\begin{align}
\begin{array}{lll}
&w_1^T(s)\left(
\begin{array}{c}
    0 \\
    1 
    \end{array}
  \right)M_L&=0 \rightarrow 
  w_1^T(s)\left(
\begin{array}{cc }
    0 & 1 
    \end{array}
  \right)
   \ \ \ &({\rm independent\  on}\  M_L)\\
&w_2^T(s)\left(
\begin{array}{c}
    1 \\
    0 
    \end{array}
  \right)U_A&=0 \rightarrow 
  w_2^T(s)\left(
\begin{array}{cc }
    1 & 0 
    \end{array}
  \right)
   \ \ \ &({\rm independent\  on} \ U_A)\\
&w_3^T(s)\left(
\begin{array}{c}
    R_A+L_As \\
    -\Psi
    \end{array}
  \right)I_A&=0 \rightarrow 
  w_3^T(s)\left(
\begin{array}{cc }
     -\Psi & R_ALL_As 
    \end{array}
  \right)
   \ \ \ &({\rm independent\  on}\  I_A)\\
&w_4^T(s)\left(
\begin{array}{c}
    \Psi \\
    M_{F1}+Js
    \end{array}
  \right)\omega&=0 \rightarrow 
  w_4^T(s)\left(
\begin{array}{cc }
    M_{F1}+Js  &-\Psi  
    \end{array}
  \right)
   \ \ \ &({\rm independent\  on}\  \omega)
\end{array}
\end{align}
すなわち、 W(s)
 \begin{align}
W=\left(
\begin{array}{c}
    w_1(s) \\
    w_2(s) \\
    w_3(s) \\
    w_4(s) 
    \end{array}
  \right) 
   =\left(
\begin{array}{cc}
    0 & 1 \\
    1 & 0\\
    -\Psi & R_A+L_As \\
    M_{F1}+Js  &-\Psi \\
    \end{array}
  \right)
\end{align}
となり、それぞれの入出力に独立な残差が求められます。

付録2 シミュレーションコード

パラメータ変動の部分で2つのシミュレーションコードに若干の違いがありますので注意してください。いろいろ関数化しようと思ったけど怠けてべた書きにしちゃいました。間違いあったら教えてください。カルマンフィルタをきちんと使う場合はQとRを共分散を計算して使いましょう。

1. R_A故障(\Delta R_A:+50\%)でのパラメータ推定シミュレーション
 

close all
clear 
clc

Ts = 1e-4; % sampling time
Tsim=6; % simulation time 6sec
t=0:Ts:Tsim-1*Ts; % time
Nsim=Tsim/Ts; % number of step

% original plant parameter
RA_0  = 1.52;
LA_0  = 6.82e-3;
Psi_0 = 0.33;
KB_0  = 2.21e-3;
J_0   = 1.92e-3;
MF1_0 = 0.36e-3;
MF0_0 = 0.11;

% current plant parameter
RA  = RA_0;
LA  = LA_0;
Psi = Psi_0;
KB  = KB_0;
J   = J_0;
MF1 = MF1_0;
MF0 = MF0_0;

% continuous state matrix of plant
A = [-RA_0/LA_0 -Psi_0/LA_0
     Psi_0/J_0  -MF1_0/J_0];
B = [1/LA_0 0
     0 -1/J_0];

% original discrete state matrix of plant
Ad_0 = eye(2)+A*Ts;
Bd_0 = B*Ts;

% current discrete state matrix of plant
Ad = Ad_0;
Bd = Bd_0;

% init variables
x(1:2,1:Nsim)=0;
y(1:2,1:Nsim)=0;
xdot(1:2,1:Nsim)=0;
yddot(1:2,1:Nsim)=0;
ydot(1:2,1:Nsim)=0;
yddot(1:2,1:Nsim)=0;

% init inputs
u(1:2,1:Nsim)=0;
udot(1:2,1:Nsim)=0;
ML = 0;
UA(1:Nsim)=0;

% definition of input ua
ua(1:Nsim)  = 0;
ua(Nsim/8+1:Nsim/8*2)= 1;
ua(Nsim/8*2+1:Nsim/8*3)= 0;
ua(Nsim/8*3+1:Nsim/8*4)= 1;
ua(Nsim/8*4+1:Nsim/8*5)= 0;
ua(Nsim/8*5+1:Nsim/8*6)= 1;
ua(Nsim/8*6+1:Nsim/8*7)= 0;
ua(Nsim/8*7+1:Nsim/8*8)= 1;
ua(1:Nsim) = (ua(1:Nsim)+(rand(1,Nsim)*1-0.5)*0.5*0);
ua(1:Nsim) =ua(1:Nsim)*15;

% init r
r1(1,1:Nsim) = 0;
r2(1,1:Nsim) = 0;
r3(1,1:Nsim) = 0;
r4(1,1:Nsim) = 0;

% init estimated state variable of extended kalman filter 
xh(1:3,1:Nsim)=0;
xh_(1:3,1:Nsim)=0;

% init estimated RA time data
RAest(1:Nsim)  = 0;
% init actual RA time data
RAactual(1:Nsim)  = 0;

P = eye(3);
Q = diag([1 1 1]);
R = diag([1 1]);


% Simulation loop
for i=3:Nsim
    
    % output of system
    y(:,i) = x(:,i);
    ydot(:,i) = (y(:,i)-y(:,i-1))/Ts;
    yddot(:,i) = (ydot(:,i)-ydot(:,i-1))/Ts;
    udot(:,i)  = (u(:,i-1)-u(:,i-2))/Ts;
    
    % residuals from parity equation
    r1(i) = LA_0*ydot(1,i)+RA_0*y(1,i)+Psi_0*y(2,i)-u(1,i-1);
    r2(i) = J_0*ydot(2,i)-Psi_0*y(1,i)+MF1_0*y(2,i)+ML;
    r3(i) = J_0*LA_0*yddot(1,i)+(LA_0*MF1_0+J_0*RA_0)*ydot(1,i)+(Psi_0*Psi_0+RA_0*MF1_0)*y(1,i) ...
        -J_0*udot(1,i)-MF1_0*u(1,i-1)-Psi_0*u(2,i-1);
    r4(i) = J_0*LA_0*yddot(2,i)+(LA_0*MF1_0+J_0*RA_0)*ydot(2,i)+(Psi_0*Psi_0+RA_0*MF1_0)*y(2,i) ...
        -Psi_0*u(1,i-1)+LA_0*udot(2,i)+RA_0*u(2,i-1);
    
    % exptended kalman filter
    Ajac = [xh(3,i-1) Ad_0(1,2) xh(1,i-1)
           Ad_0(2,1) Ad_0(2,2) 0
           0  0 1];
    Cjac = [1 0 0
           0 1 0];
    xh_(:,i) = Ajac*xh(:,i-1)+[Bd_0;0 0]*(u(:,i-1))-[xh(1,i-1)*xh(3,i-1);0;0];
    P_  = Ajac*P*Ajac'+Q;
    
    G = P_*Cjac'/(Cjac*P_*Cjac'+R);
    xh(:,i) = xh_(:,i)+G*(y(:,i)-Cjac*xh_(:,i));
    P = (eye(3)-G*Cjac)*P_;
    
    % estimated Ad and A matrixs
    Adest = [xh(3,i-1) Ad_0(1,2)
             Ad_0(2,1) Ad_0(2,2)];
    Aest = (Adest-eye(2))/Ts;
    
    % time data of estimated RA
    RAest(i)  = -Aest(1,1)/B(1,1);
    
    
    % if time is bigger than 4sec, RA is multiplied by 1.5. 
    if i<(Nsim*4/6)
        RA  = RA_0;     % RA without error 
    else
        RA  = RA_0*1.5; % RA with error 
    end
    
    % time data of RA without RA variation
    RAactual(i) = RA;
    
    % matrix is updated.
    A = [-RA/LA -Psi/LA
        Psi/J  -MF1/J];
    B = [1/LA 0
        0 -1/J];
    
    % calculation of discrete matrix
    Ad = eye(2)+A*Ts;
    Bd = B*Ts;
    
    % caluculation of inputs
    ML = MF0*sign(x(2,i));
    UA(i) = ua(i)-KB*abs(x(2,i))*x(1,i) + (rand()*1-0.5)*3*1;
    u(:,i) = [UA(i);ML];
    
    if i==Nsim
        % do nothing
    else
        x(:,i+1) = Ad*x(:,i)+Bd*u(:,i);
    end
    
end

% plot inputs and outputs of plant
figure
subplot(2,2,1)
plot(t,x(1,:))
grid on
hold on
title('Current {I_A} with \Delta R_A')
xlim([2 6])
ylim([-10 10])

subplot(2,2,2)
plot(t,x(2,:))
grid on
hold on
title('Speed \omega with \Delta R_A')
xlim([2 6])
ylim([0 45])

subplot(2,2,3)
plot(t,ua)
hold on
grid on
title('Valtage u_{a} with \Delta R_A')
xlim([2 6])
ylim([0 20])

subplot(2,2,4)
plot(t,UA)
hold on
grid on
title('Valtage U_{A} with \Delta R_A')
xlim([2 6])
ylim([0 20])

% plot actual and estimated RA time data
figure
hold on
grid on
plot(t,RAactual(:),'g-','LineWidth',2.5)
plot(t,RAest(:))
xlim([2 6])
ylim([0 10])
title('Estimated R_A with \Delta R_A')
legend({'Actual R_A','Estimated R_A'},'Orientation','horizontal')
text(3.0,5,'\Delta R_A: 0% \leftarrow','Fontsize',15 )
text(4.05,5,'\rightarrow \Delta R_A: +50%','Fontsize',15)

% plot residuals
figure
subplot(2,2,1)
plot(t,r1)
grid on
hold on
plot([4 4],[-100 100],'k--')
title('r1 with \Delta R_A')
ylim([-6 4])
xlim([2 6])
text(4.2,-5,'変動 大','Fontsize',12)

subplot(2,2,2)
plot(t,r2)
grid on
hold on
plot([4 4],[-100 100],'k--')
title('r2 with \Delta R_A')
ylim([-0.1 0.1])
xlim([2 6])
text(4.2,-4,'変化なし','Fontsize',12)

subplot(2,2,3)
plot(t,r3)
grid on
hold on
plot([4 4],[-100 100],'k--')
title('r3 with \Delta R_A')
ylim([-10 10])
xlim([2 6])
text(4.2,7,'変動 大','Fontsize',12)

subplot(2,2,4)
plot(t,r4)
grid on
hold on
plot([4 4],[-100 100],'k--')
title('r4 with \Delta R_A')
ylim([-10 10])
xlim([2 6])
text(4.2,8,'変動 大','Fontsize',12)


2.電圧測定センサーゲインU_Aの故障(\Delta U_A:+10\%)でのパラメータ推定シミュレーション
 

close all
clear 
clc

Ts = 1e-4; % sampling time
Tsim=6; % simulation time 6sec
t=0:Ts:Tsim-1*Ts; % time
Nsim=Tsim/Ts; % number of step

% original plant parameter
RA_0  = 1.52;
LA_0  = 6.82e-3;
Psi_0 = 0.33;
KB_0  = 2.21e-3;
J_0   = 1.92e-3;
MF1_0 = 0.36e-3;
MF0_0 = 0.11;

% current plant parameter
RA  = RA_0;
LA  = LA_0;
Psi = Psi_0;
KB  = KB_0;
J   = J_0;
MF1 = MF1_0;
MF0 = MF0_0;

% continuous state matrix of plant
A = [-RA_0/LA_0 -Psi_0/LA_0
     Psi_0/J_0  -MF1_0/J_0];
B = [1/LA_0 0
     0 -1/J_0];

% original discrete state matrix of plant
Ad_0 = eye(2)+A*Ts;
Bd_0 = B*Ts;

% current discrete state matrix of plant
Ad = Ad_0;
Bd = Bd_0;

% init variables
x(1:2,1:Nsim)=0;
y(1:2,1:Nsim)=0;
xdot(1:2,1:Nsim)=0;
yddot(1:2,1:Nsim)=0;
ydot(1:2,1:Nsim)=0;
yddot(1:2,1:Nsim)=0;

% init inputs
u(1:2,1:Nsim)=0;
udot(1:2,1:Nsim)=0;
ML = 0;
UA(1:Nsim)=0;

% definition of input ua
ua(1:Nsim)  = 0;
ua(Nsim/8+1:Nsim/8*2)= 1;
ua(Nsim/8*2+1:Nsim/8*3)= 0;
ua(Nsim/8*3+1:Nsim/8*4)= 1;
ua(Nsim/8*4+1:Nsim/8*5)= 0;
ua(Nsim/8*5+1:Nsim/8*6)= 1;
ua(Nsim/8*6+1:Nsim/8*7)= 0;
ua(Nsim/8*7+1:Nsim/8*8)= 1;
ua(1:Nsim) = (ua(1:Nsim)+(rand(1,Nsim)*1-0.5)*0.5*0);
ua(1:Nsim) =ua(1:Nsim)*15;

% init r
r1(1,1:Nsim) = 0;
r2(1,1:Nsim) = 0;
r3(1,1:Nsim) = 0;
r4(1,1:Nsim) = 0;

% init estimated state variable of extended kalman filter 
xh(1:3,1:Nsim)=0;
xh_(1:3,1:Nsim)=0;

% init estimated RA time data
RAest(1:Nsim)  = 0;
% init actual RA time data
RAactual(1:Nsim)  = 0;

P = eye(3);
Q = diag([1 1 1]);
R = diag([1 1]);


% Simulation loop
for i=3:Nsim
    
    % if time is bigger than 4sec, UAgain becomes 1.1 times. 
    if i<(Nsim*4/6)
        UAgain = 1;     % UA gain without gain error
    else
        UAgain = 1.1;   % UA gain with gain error
    end
    
    % output of system
    y(:,i) = x(:,i);
    ydot(:,i) = (y(:,i)-y(:,i-1))/Ts;
    yddot(:,i) = (ydot(:,i)-ydot(:,i-1))/Ts;
    udot(:,i)  = (u(:,i-1)-u(:,i-2))/Ts;
    
    % residuals from parity equation
    r1(i) = LA_0*ydot(1,i)+RA_0*y(1,i)+Psi_0*y(2,i)-u(1,i-1)*UAgain;
    r2(i) = J_0*ydot(2,i)-Psi_0*y(1,i)+MF1_0*y(2,i)+ML;
    r3(i) = J_0*LA_0*yddot(1,i)+(LA_0*MF1_0+J_0*RA_0)*ydot(1,i)+(Psi_0*Psi_0+RA_0*MF1_0)*y(1,i) ...
        -J_0*udot(1,i)*UAgain-MF1_0*u(1,i-1)*UAgain-Psi_0*u(2,i-1);
    r4(i) = J_0*LA_0*yddot(2,i)+(LA_0*MF1_0+J_0*RA_0)*ydot(2,i)+(Psi_0*Psi_0+RA_0*MF1_0)*y(2,i) ...
        -Psi_0*u(1,i-1)*UAgain+LA_0*udot(2,i)+RA_0*u(2,i-1);
    
    % exptended kalman filter
    Ajac = [xh(3,i-1) Ad_0(1,2) xh(1,i-1)
           Ad_0(2,1) Ad_0(2,2) 0
           0  0 1];
    Cjac = [1 0 0
           0 1 0];
    xh_(:,i) = Ajac*xh(:,i-1)+[Bd_0;0 0]*(u(:,i-1).*[UAgain;1])-[xh(1,i-1)*xh(3,i-1);0;0];
    P_  = Ajac*P*Ajac'+Q;
    
    G = P_*Cjac'/(Cjac*P_*Cjac'+R);
    xh(:,i) = xh_(:,i)+G*(y(:,i)-Cjac*xh_(:,i));
    P = (eye(3)-G*Cjac)*P_;
    
    % estimated Ad and A matrixs
    Adest = [xh(3,i-1) Ad_0(1,2)
             Ad_0(2,1) Ad_0(2,2)];
    Aest = (Adest-eye(2))/Ts;
    
    % time data of estimated RA
    RAest(i)  = -Aest(1,1)/B(1,1);
    
    
    % time data of RA without RA variation
    RAactual(i) = RA;
    
    % matrix is updated.
    A = [-RA/LA -Psi/LA
        Psi/J  -MF1/J];
    B = [1/LA 0
        0 -1/J];
    
    % calculation of discrete matrix
    Ad = eye(2)+A*Ts;
    Bd = B*Ts;
    
    % caluculation of inputs
    ML = MF0*sign(x(2,i));
    UA(i) = ua(i)-KB*abs(x(2,i))*x(1,i) + (rand()*1-0.5)*3*1;
    u(:,i) = [UA(i);ML];
    
    if i==Nsim
        % do nothing
    else
        x(:,i+1) = Ad*x(:,i)+Bd*u(:,i);
    end
    
end

% plot inputs and outputs of plant
figure
subplot(2,2,1)
plot(t,x(1,:))
grid on
title('Current {I_A} with \Delta U_Again')
xlim([2 6])
%ylim([0 1])

subplot(2,2,2)
plot(t,x(2,:))
grid on
title('Speed \omega with \Delta U_Again')
xlim([2 6])
ylim([0 45])

subplot(2,2,3)
hold on
plot(t,ua)
grid on
title('Valtage u_{a} with \Delta U_Again')
xlim([2 6])
ylim([0 20])

subplot(2,2,4)
hold on
plot(t,UA)
grid on
title('Valtage U_{A} with \Delta U_Again')
xlim([2 6])
ylim([0 20])


% plot actual and estimated RA time data
figure
hold on
grid on
plot(t,RAactual(:),'g-','LineWidth',2.5)
plot(t,RAest(:))
plot([4 4],[0 10],'k--')
xlim([2 6])
ylim([0 10])
title('Estimated R_A with \Delta U_Again')
legend({'Actual R_A','Estimated R_A'},'Orientation','horizontal')
text(3.0,9,'\Delta U_A: 0% \leftarrow','Fontsize',15 )
text(4.05,9,'\rightarrow \Delta U_A: +10%','Fontsize',15)
text(4.5,8,'真値と推定値が異なる','Fontsize',12)


% plot residuals
figure
subplot(2,2,1)
plot(t,r1)
grid on
hold on
plot([4 4],[-100 100],'k--')
title('r1 with \Delta U_Again')
ylim([-2 1])
xlim([2 6])
text(4.05,-1.8,'負の値に偏差','Fontsize',12)
text(4.2,0.4,'変動 大','Fontsize',12)

subplot(2,2,2)
plot(t,r2)
grid on
hold on
plot([4 4],[-100 100],'k--')
title('r2 with \Delta U_Again')
ylim([-0.1 0.1])
xlim([2 6])
text(4.05,-0.07,'変化なし','Fontsize',12)

subplot(2,2,3)
plot(t,r3)
hold on
plot([4 4],[-100 100],'k--')
grid on
title('r3 with \Delta U_Again')
ylim([-10 10])
xlim([2 6])
text(4.2,8,'変動 大','Fontsize',12)

subplot(2,2,4)
plot(t,r4)
grid on
hold on
plot([4 4],[-100 100],'k--')
title('r4 with \Delta U_Again')
ylim([-5 5])
xlim([2 6])
text(4.05,-4,'負の値に偏差','Fontsize',12)