はじめに
パラメータ推定（１）では，線形最小二乗法によるパラメータ推定について述べてみました．今回は拡張カルマンフィルター（EKF：Extended Kalman Filter）と呼ばれる手法によるパラメータ推定について書きます．
線形最小二乗法は非常に使いやすい推定法ですが，その欠点として
- 微分値の計算が厄介（別途フィルタリングの設計などが必要）
- 非線形システムに対応してない
などがあります．これをEKFで解決します．
※カルマンフィルターの概要を書いていたらだいぶ長くなってしまったのですが，流れをまとめると
- モデル化と状態方程式：カルマンフィルターは状態方程式に基づいて設計されので，まずはここから．
- 離散化とノイズ：これらが加わったときにシステムがどうのように記述されるか，について
- カルマンフィルタのアルゴリズム：一応書いた方がいいかなと思ったのですが，ほかの方が書いている記事の方が分かりやすいと思います．
- 外乱オブザーバ：パラメータ推定のアイデアの元となるものなので，一緒に説明しておきます．
- パラメータ推定：やっと！
って感じになってます．カルマンフィルタを知ってる人は上３つは飛ばしてもらっても大丈夫です．
カルマンフィルターって？
カルマンフィルターとはルドルフ・カルマンによって提案された，動的システム（運動方程式に微分が出てくるシステム）の状態量推定に用いられる手法です．知ってる人は，この章は読み飛ばしてください．
このカルマンフィルターはNASAのアポロ計画に使われたことでも有名です．カルマンフィルターって何？という説明は，下記リンクの動画とかが非常に分かりやすかったです．
Understanding Kalman Filters (Mathworks)
https://jp.mathworks.com/videos/understanding-kalman-filters-part-1-why-use-kalman-filters--1485813028675.html
ちなみに，制御の世界では状態量を求める手法としてオブザーバというものが存在します．オブザーバの中で，ノイズの特性や，初期推定値の分散などを考慮した最適なオブザーバがカルマンフィルタだと思っておけば大丈夫です．
状態方程式
カルマンフィルタは状態方程式と呼ばれる形式でシステムを記述するところから始まります．数式で簡単に見てみましょう．
パラメータ推定（１）でも取り扱った，マスダンパ系で考えます．ただ今回は位置$p$も考慮して，次のような運動方程式を取り扱います．
$$ m \ddot p + b \dot p = u $$
$m$が質量，$b$が粘性抵抗係数，$p$が物体の位置，$u$が物体に働く力です（力を$f$で書こうと思ったのですが，$f$は別のところで使うので，今回は入力$u$でいきます）．
この運動方程式を状態方程式と呼ばれる形式に書き直します．なんでそんなことするの？と思う方もいるかもしれませんが，２階微分が出てくるシステムは数学的に扱いづらいので，全部１階微分に書き換えてしまえ，的な発想なのです（私も初めて制御工学を習ったときは戸惑いましたが，慣れると普通に感じます）．
まず位置の微分は速度なので，$\dot p$を速度$v$とおきましょう．
$$\dot p = v$$
次に，$v$の微分は$\dot v=\ddot p$なので，運動方程式から
$$ \dot v = -\frac{b}{m}v + \frac{1}{m}u $$
と書けます．この二つの式を組み合わせると
\frac{d}{dt}
\begin{bmatrix}
p \\ v
\end{bmatrix}
=\begin{bmatrix}
v \\ -\frac{b}{m}v
\end{bmatrix}
+\begin{bmatrix}
0 \\ \frac{1}{m}
\end{bmatrix}
u
という形にまとめることができます．式が二本に増えてしまいましたが，この式には微分が１階しか出てこないので，制御的に非常に扱いやすくなります．今回はシステムが線形なので，ここから更に線形システムの一般系として次のように書くことができます．
\dot x = Ax + Bu ,\\[10pt]
x = \begin{bmatrix}
p \\ v
\end{bmatrix},
\ A = \begin{bmatrix}
0 & 1 \\
0 & -\frac{b}{m}
\end{bmatrix},
\ B = \begin{bmatrix}
0 \\ \frac{1}{m}
\end{bmatrix}
$x$は状態量ベクトル，$[A,\ B]$は線形係数行列みたいに言ったりもします．状態量$x$の変化を記述する式なので，状態方程式と言うんですね．観測量についてもシステムとして記述します．今回は位置$p$のみが観測できる値であるとして考えれば，観測量$y$と状態量$x$との関係は以下に示す線形な等式条件で書くことができます．
y=Cx,\
\ C=\begin{bmatrix}
1 & 0
\end{bmatrix}
この式のことを観測方程式と呼びます．
離散化
一般的に使われているカルマンフィルターは離散システムに対して設計されます．なので，今回の$\dot x = Ax + Bu$も離散化してあげないとダメです．離散化されたシステムは一般的に
$$ x_{k+1}=A_d x_k + B_d u_k $$
の形で書かれます．先の節で求めた状態方程式と比較すると，左辺が$\dot x$から$x_{k+1}$に変わっています．時間ステップ$k$の時の状態$x_k$から，次のステップ$k+1$における状態$x_{k+1}$を求める式ですね．係数も$A_d$のように$d$が添え字についています．これは離散化：discretizationの$d$です．
では，先ほど求めた$A$と$B$から$A_d$と$B_d$を求めてみます．非常に短い時間$dt$を考えると，連続の状態方程式$\dot x=Ax+Bu$から，以下の式が作れます
$$\frac{x(t_{k+1}) - x(t_k)}{dt}=Ax(t_k)+Bu(t_k) $$
左辺は$dt \rightarrow 0$の極限で$\dot x$に収束するので，理にかなっていますね．ここで$x(t_k)=x_k$などと定義して式変形すると
$$ x_{k+1}=(I+Adt)x_k + (Bdt)u_k $$
となるので（$I$は単位行列），結果的に離散化されたシステム$A_d$，$B_d$と連続なシステム$A$，$B$との間には
$$ A_d = I+Adt,\ \ B_d = Bdt $$
の関係があることが分かりました．$y=Cx$に関してはただの静的拘束なので，特に変化はないです．ただ，見にくいので一応$C_d=C$として$C_d$を定義しておきましう．
※ちなみに，ここで用いた離散化は前進差分と呼ばれるもので，そのほかにも後退差分や中央差分，双一次変換など様々な離散化の手法があり，手法に応じて上で求めた$A_d$と$A$の関係式が変化します．一般的に前進差分等よりも双一次変換などの方が離散化の精度が高くなります．
##ノイズ
先ほど離散化されたシステム$x_{k+1}=A_dx+B_du_k$を作りました．しかし，世の中はノイズやら，雑な近似にまみれているのです．そんな汚れちまった世の中じゃ，システムはなかなか理想通り動いてくれません．その点を考慮するために，ノイズを式に付け足してやりましょう．すると，先ほどまでの式は
$$ x_{k+1}=A_dx_k+B_du + v_k(0, Q) $$
となります．いきなり現れた$v_k$がノイズです．カルマンフィルタではノイズの確率密度は平均が0の白色ガウス分布であると仮定しています．ノイズの白色性というのは，ノイズ強度がどの周波数帯でも一様，ガウス分布とは正規分布に従うということです．ガウス分布は平均と分散からその特性が決定され，今回は平均$0$，分散（多次元なので共分散行列）$Q$を引数に持っており，この$Q$が今回のノイズの特性を決定します．
このシステムにはどんなノイズが入るの？と聞かれた場合は，平均0，共分散行列$Q$をもつ白色ガウスノイズだよ，と言えば一意にノイズの性質が伝わるのです．
同様に，観測時にもノイズが入ります．なので，観測方程式は
$$ y_k=C_dx_k + w_k(0,R) $$
となります．なお，$v_k$がシステムが遷移するプロセスに対して生じるノイズなのでプロセスノイズ，$w_k$が観測時に入るノイズなので観測ノイズと呼ばれます．
カルマンフィルタのアルゴリズム
ここまでをまとめると，
- システムの特性は$A_d$，$B_d$，$C_d$に
- ノイズの特性は$Q$，$R$に
集約されたことが分かります．カルマンフィルタは，これらの情報をまとめて最も最適な推定値を計算します．例えば，異常に$Q$が大きい場合は，システムモデル$A_d$，$B_d$は全く信頼できない値となるので，観測された値$y_k$を信じます．一方で$R$が大きい場合は，観測した値$y_k$は信頼できないので，これまでに推定した結果$x_{k}$から$A_d$，$B_d$を使って$x_{k+1}$を計算し，その値を真の値だと考えるのです．
この辺の説明は別の方が書かれていた記事の説明が非常に分かりやすかったです．
Qiita：シンプルなモデルとイラストでカルマンフィルタを直感的に理解してみる
http://qiita.com/MoriKen/items/0c80ef75749977767b43
では，実際のアルゴリズムについて書きましょう．カルマンフィルタのアルゴは
- 予測ステップ
- 更新ステップ
の２段階に分かれています．イメージとして，まず現時点での$x_k$の推定値$\hat x_k$が得られており，その情報から離散状態方程式を用いて次のステップでの$\hat x_{k+1}$を予測します（予測ステップ）．そのあとに，実際に得られた測定値から，予測していた推定値の補正を掛けます（更新ステップ）．
カルマンフィルタの説明のときは，この辺の記法が非常にややこしいのですが，$\hat x$のように，頭にハットがついているものがカルマンフィルタによる推定値で，これから式にたくさん出てくる$\hat x_{k+1|k}$は，$k$ステップ時に予測計算した$k+1$ステップ時の推定値です．とりあえず，予測してみましょう．式はこんな感じになります．
予測ステップ\begin{align}
\hat x_{k+1|k} &=A_d\hat x_{k|k} + B_du_k,\\
P_{k+1|k} &= A_d P_{k|k}A_d^T + Q
\end{align}
ここで出てきた$P$という値は，予測誤差の共分散行列と呼ばれるもので，今現在の推定値の精度（共分散）を示す行列になります．初期値$P_{0|0}$を与えてやれば，通常は徐々に$P_{k|k}$は一定の値に減少していき，最終的に一定値に落ち着きます（収束値は$A_d, B_d, C_d, Q, R$から一位に定まります）．
この予測ステップで行っていることは，$x_{k+1}=A_dx_k+B_du_k$の式を使って，次のステップでの状態量がどうなるかを計算する，でもノイズが入る可能性があるので，予測誤差も$P_{k+1|k}$として計算しておく，といった感じです．
ただ，ここで行っていることはあくまで予測なので，これが推定値になるわけではありません．
また，実際の観測情報は何も使っていないので，このステップにおける誤差$P$の値は上昇します．
（$P_{k+1|k}$の計算における第1項$A_d P_{k|k}A_d^T$は離散化による誤差項，第2項$+Q$はプロセスノイズによる誤差項です．）
次に更新ステップです．上の予測ステップでは，$k$から$k+1$を「予測」しましたが，次は$1$ステップ進んで「更新」をかけます．その時に用いる値が，1ステップ前で予測した状態量になります．なので，先ほど予測した$x_{k+1|k}$は，このステップでは$x_{k|k-1}$として扱われます．
更新ステップ\begin{align}
K_k &= P_{k|k-1} C_d^T(C_d P_{k|k-1} C^T+R)^{-1} \\
\hat x_{k|k} &= \hat x_{k|k-1} + K_k(y_k - C_d \hat x_{k|k-1}) \\
P_{k|k} &= (I-K_kC_d)P_{k|k-1}
\end{align}
ここでは観測値を使った予測の修正が行われています．１行目はカルマンゲインと呼ばれる行列の計算，２行目で測定結果を加味した推定値の更新，３行目で，推定値を更新した上での推定共分散行列の計算を行っています．ここでの$x_{k|k}$の値が最終的に$k$ステップでのカルマンフィルタによる推定値になります．なお，実際の推定値という新しい情報を使っているので，このステップでは一般的に誤差の分散は減少します．
このアルゴリズムを使えば，各ステップで最適な推定値を計算することができます．しかも，実際に測定した値は位置$p$だけであるにも関わらず，その微分値$v$も一緒に状態量$x$として推定することができます．
これ，すごくないですか？
なぜ最適になるかは置いておくとして，位置しか測定してないのに，速度も勝手に推定できてしまうんです．まあ，今回はスカラで書き下すと結局やっていることは差分して移動平均フィルタをかけているだけでしょうけど，もっとすごいこともできます．例えば，物体Aと物体Bがバネでつながったシステムというのを考えてみます．カルマンフィルタを使えば，物体Aの位置を測定するだけで，Aの速度，Bの位置，Bの速度のすべてが計算できてしまいます．
これの応用例は車とかの複雑な機械システムです．一般的に車のエンジンは高温や高振動のため，センサーを付けることができません．なので，タイヤの回転数と車速などといった情報から間接的に，エンジントルクなどといったセンサーで直接図ることができない値を推定できたりします．
なんで物体Aの位置しか測ってないのに，Bの位置も速度も分かるんや，という質問に答えるには，離散系を取り扱うカルマンフィルタよりは，連続系を対象にしたオブザーバの設計や概念を学んだ方が分かりやすいと思います．
話がそれました．カルマンフィルタに戻ります．
ここで注意なのですが，カルマンフィルタの言う最適というのは「平均0の白色ガウスノイズが印加されるシステムに対して，システムのモデル$A_d$，$B_d$，$C_d$および，プロセスノイズと観測ノイズの共分散$Q$，$R$を考慮した最適推定値」という意味であり，$A_d$や$Q$が事前に分かっていないといけません．一般的に$Q$や$R$は簡単に求められるものではなく，また多次元システムになると$A_d$を計算するだけでも一苦労だったりします．その辺が雑になっていると，たとえカルマンフィルタを作ったとしても，物理的に何も意味のない最適値になってしまうので注意が必要です．たまに「$Q$，$R$は単位行列とした」的な表記を見かけますが，それだとわざわざカルマンフィルタを使う意味がなくなってしまいます．実際問題，これらの行列をどうやって設計すべきなの？といった問題もいまだオープンですが，いくつか王道っぽい方法も存在します．論文だと例えば
"How To NOT Make the Extended Kalman Filter Fail", Industrial & Engineering Chemistry Research (2013)
とかに書いてあります．論文のタイトルが超ダイレクトですね．笑
カルマンフィルタのアルゴリズム自体は非常に簡単なものなので，だれにでも容易に実装できるというメリットがあります．ただ，その裏にある条件や仮定を知らないと性能が十分発揮されないケースがあるので，ご注意ください．設計の前に「それほんとにガウスノイズなの？」とか，「共分散行列の与え方はそれでいいの？」とか考えてみてください．もしノイズが白色ノイズでない場合も，次に述べる外乱オブザーバなどで対処できる可能性があります．
外乱オブザーバ
カルマンフィルタを使って，状態量が推定できることが分かりました．ただ，ノイズの平均が常に0だとは限りません．一定のバイアスが載った外乱が入ってくることも考えられます（摩擦とかはその一種ですね）．この外乱をカルマンフィルターの仮定に当てはめるのはちょっと無理があります．そういった場合に対応するのが，外乱オブザーバです．
カルマンフィルタの流れは，
1. 線形のシステム$x_{k+1}=A_dx_k+B_ku$，$y_k=C_dx_k$を用意する．
2. ノイズ特性を$Q$，$R$として与える．
3. $x$の推定値が求まる
でした．今回はシステムの形をちょこっと変形して，外乱オブザーバを設計します．
一番初めに設計した，マスダンパシステムを思い出します．
\begin{align}
\dot p &= v, \\
\dot v &= -\frac{b}{m}v + \frac{1}{m}u
\end{align}
これです．しかし，実はここに一定の摩擦力$d$が働いていたとしましょう．すると式は次のようになります．
\begin{align}
\dot p &= v, \\
\dot v &= -\frac{b}{m}v + \frac{1}{m}(u + d),\\
d&=const.
\end{align}
入力の部分に$d$が追加されました．これは一定の値なので，$d=const$が３行目に追加されています．もしこの値$d$を推定することが出来たら気持ちいいですね．推定した摩擦の値を入力に付け足してやれば，摩擦を打ち消した状態での制御が可能になります．やってみましょう．
カルマンフィルタが推定できるのは状態量$x$でした．今回は外乱$d$も推定したい値なので，まとめて$d$もシステムの状態として記述してやります．つまり$x=[p,\ v,\ d]^T$になるわけです．もちろん，それに伴ってシステムの係数行列も変わります．状態方程式を書くためには，状態量の微分値の情報が必要です．では$\dot d$は何でしょうか？答えは$0$です．過程として$d$は定数と与えているので，$\dot d=0$が状態方程式に加わります．
幸いなことにこの状況でもシステムはまだ線形でいてくれるので，線形状態方程式として外乱を含むシステムを書くと
\frac{d}{dt}
\begin{bmatrix}
p \\ v \\ d
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 & 0 \\
0 & -\frac{b}{m} & \frac{1}{m} \\
0 & 0 & 0
\end{bmatrix}
\begin{bmatrix}
p \\ v \\ d
\end{bmatrix}
+
\begin{bmatrix}
0 \\ \frac{1}{m} \\ 0
\end{bmatrix}
u
となります．これはもう，ちゃんとした状態方程式ですね．あとはこれをシステムモデルとしてカルマンフィルターに入れれば，外乱$d$が定数として推定されるわけです．
※もちろん，システムが大きく変わっているので，これに応じて$C_d$，$Q$，$R$も設計しなおす必要があります．
EKFによるパラメータ推定
いよいよ大詰めです！パラメータ推定に行ってみましょう．
おさらいになりますが，
カルマンフィルタではマスダンパ系に白色ガウスノイズが入ってくる場合の最適推定値を計算するものでした．
外乱オブザーバは，カルマンフィルタでは対応しきれない定常外乱に対応します．
次に考えるのはシステムのパラメータが分からない場合です．
粘性係数$b$はこれまで既知であると考えてきましたが，この値はそんな簡単に求まるもんじゃないです．ちゃんとパラメータ推定しないといけません．
しかも，粘性係数は温度とかの環境で容易に変化する値です．そんな値を本当に定数として取り扱ってしまっていいのでしょうか？できればオンラインで常に粘性係数の推定値を更新し続けれれば最高です．
ということで，カルマンフィルタによるパラメータ推定を始めましょう！
今回は粘性抵抗係数$b$を未知パラメータとしてシステムを作ってやります．
どうするかは簡単です．$b$も定数として状態量に入れてやるのです．
ですが，先ほど言った通り$b$は温度とかで変動する値です．なので，実際にはこのモデルは間違っています．
この「間違っている」という情報は，プロセスノイズである$Q$に入れることができます．
なので，適切に$Q$を設定してあげれば，うまく$b$の変動をカルマンフィルターを使って推定することができるのです．
$b$を情報量に入れると，状態方程式は（今回は行列の形式で書いていません）
\frac{d}{dt}
\begin{bmatrix}
p \\ v \\ d \\ b
\end{bmatrix}
=
\begin{bmatrix}
v \\
-\frac{b}{m}v + \frac{1}{m}d\\
0 \\ 0
\end{bmatrix}
+
\begin{bmatrix}
0 \\ \frac{1}{m} \\ 0 \\0
\end{bmatrix}
u
こうなります．時間幅$dt$で離散化すると
\begin{bmatrix}
p_{k+1} \\ v_{k+1} \\ d_{k+1} \\ b_{k+1}
\end{bmatrix}
=
\begin{bmatrix}
p_k + v_kdt \\
v_k +(-\frac{b_k}{m}v_k + \frac{1}{m}d_k)dt\\
d_k \\ b_k
\end{bmatrix}
+
\begin{bmatrix}
0 \\ \frac{1}{m}dt \\ 0 \\ 0
\end{bmatrix}
u_k
となります．外乱オブザーバの場合はこれでよかったんですが，今回は少し厄介です．右辺第１項の２行目に$b_k*v_k$を含む項があります．これは状態量の成分同士の積（$b_k=x_k(4), v_k=x_k(2)$）になるので，システムは非線形になります．そのため，これまで使ってきた線形のカルマンフィルターが使えなくなってしまいます．これを解決するのが拡張カルマンフィルタになります．
##拡張カルマンフィルタ
カルマンフィルターは本来，線形系を対象として設計された手法でした．
しかしカルマンがアポロ計画に参加した際に，非線形系の最適オブザーバの設計を迫られることになります．
そこで生まれたのが拡張カルマンフィルタです（この話が本当かどうかは知りません）.
拡張カルマンフィルタ（以下EKF）は，各サンプリング時間でシステムを線形近似することによって，非線形システムを強引に線形システムへと変換します．
先ほどの外乱$d$と粘性係数$b$を状態量に持つシステムは，多次元の非線形関数$F_d$を用いれば，一般系として
$$ x_{k+1}= F_d(x_k,u_k) $$
と書くことができます．今回の場合は
F_d(x_k, u_k)=
\begin{bmatrix}
p_k + v_kdt \\
v_k +(-\frac{b_k}{m}v_k + \frac{1}{m}d_k)dt + \frac{1}{m}u_kdt\\
d_k \\ b_k
\end{bmatrix}
ですね．$F_d(x_k,u_k)$自体は非線形なのですが，この微分方程式はある点$x_p, u_p$の近傍では以下のような線形システムとして振る舞いを近似することができます．
x_{k+1}=\left. \frac{\partial F_d(x,u)}{\partial x} \right|_{x=x_p,\ u=u_p} x_k +\left. \frac{\partial F_d(x,u)}{\partial u} \right|_{x=x_p,\ u=u_p} u_k
一般的なテーラー１次近似ってやつです．右辺第１項の係数行列は，$F_d$のヤコビアンなどと呼ばれます．
\left. \frac{\partial F_d(x,u)}{\partial x} \right|_{x=x_p,\ u=u_p}
この子がヤコビアンです．これ自体は定数行列とみなすことができるので，カルマンフィルタの考えが適用できるわけです．
実際に計算してみると，
\left. \frac{\partial F_d(x,u)}{\partial x} \right|_{x=x_p,\ u=u_p} =
\begin{bmatrix}
1 & dt & 0 & 0\\
0 & -\frac{b_k}{m}dt & \frac{dt}{m} & -\frac{v_k}{m}dt \\
0&0&1&0 \\
0&0&0&1
\end{bmatrix}
になります（計算が間違ってなければ）．
カルマンフィルターにおいて，予測ステップにでの状態の予測は非線形方程式を用いてそのまま計算ができるのですが，予測時の誤差の共分散行列の計算などはどうしても線形でないと解析できないため，$A_d$の代用として$F_d$のヤコビアンを用いるわけです．そのためのに，毎ステップごとで推定値$x_k$近傍でヤコビアンを計算する必要があります．
アルゴリズムをまとめまると，以下のようになります．
予測ステップ\begin{align}
\hat x_{k+1|k} &=F_d(\hat x_{k|k},u_k)\\
P_{k+1|k} &= A_d P_{k|k}A_d^T + Q
\end{align}
\begin{align}
K_k &= P_{k|k-1} C_d^T(C_d P_{k|k-1} C^T+R)^{-1} \\
\hat x_{k|k} &= \hat x_{k|k-1} + K_k(y_k - C_d \hat x_{k|k-1}) \\
P_{k|k} &= (I-K_kC_d)P_{k|k-1}
\end{align}
A_d = \left. \frac{\partial F_d(x,u)}{\partial x} \right|_{x=x_{k|k},\ u=u_k}
これらの計算を行うことによって，拡張状態量$x_k$の推定値が求まります．今回は$x_k$の4番目の値が，今回求めたいパラメータ$b$になるわけです．
さらに，パラメータの推定値の精度についても，カルマンフィルタによって計算された共分散行列$P$を用いて計算することができます．例えば$b$だけを考えた時の分散$\sigma_b$は$P(3,3)$で与えられます．よって$b$の真値は99%以上の確率で$\hat b \pm 3\sqrt{\sigma_b}$の区間に存在することが言えます．
（$P$は設計者が事前に与えた$Q,R$の情報を使って計算されるものなので，$Q,R$の値がそもそも間違っていると何とも言えませんが．．．）
#まとめ
ということで，EKFによるパラメータ推定について書いてみました．EKFは最小二乗法の欠点だった，微分値の計算，非線形への対応の課題がクリアされていますし，オンライン推定への適用も容易です．
ちょっとカルマンフィルタの説明をしすぎてパラメータ推定がしっかり説明できていない気もしますが，そこは気が向いたら手直しを加えておきます．
ソースコード
車輪移動ロボットのパラメータ推定をEKFで行いつつ，推定したパラメータを用いてLQR制御によって速度を制御してます．コードはmatlabです．
https://github.com/TakaHoribe/EKF_based_Adaptive_control