パラメータ推定（１）最小二乗法

# はじめに
パラメータ推定についてまとめる機会があったので，その辺について書いてみます．
機械やプラントの制御を行う際は，対象となるシステムのパラメータを知る必要があります．色々と手法はあるのですが，最終的に拡張カルマンフィルタ（EKF)での推定についてかけたらなぁと思ってます，

パラメータ推定はおまかに「オンライン推定」と「オフライン推定」に分かれます．EKFは後者のオンライン推定の手法になります．
今回はEKFに行く前に，最も一般的な最小二乗法による推定について書いてみます．


# 最小二乗法によるパラメータ推定
システムのパラメータ推定に関しては，最小二乗法を使うのが最も簡単な方法です．以下の一般的なマスダンパシステムで考えてみましょう．
$$m\dot v + b v = f$$

$m$が質量，$b$が粘性抵抗係数，$f$が対象に働く力，$v$が物体の速度を表しています．今回は$m$と$b$の値が分からないものとして，推定を試みます．上の式を行列で書くと


```math
\begin{bmatrix} 
\dot v & v
\end{bmatrix}
\begin{bmatrix}
m \\ b
\end{bmatrix}
=f
```

こうなります．入力$f$，速度$v$，および速度の微分値$\dot v$は基本的に測定できるものとすれば，各サンプリング点において上の式が成り立つはずです．この子たちをまとめて書くと

```math
\begin{bmatrix} 
v_1 & \dot v_1 \\
v_2 & \dot v_2 \\
v_3 & \dot v_3 \\
\vdots & \vdots
\end{bmatrix}
\begin{bmatrix}
m \\ b
\end{bmatrix}
=
\begin{bmatrix}
f_1\\f_2\\f_3 \\ \vdots
\end{bmatrix}
```

こんな感じになります．サンプリング数を$n_s$とすれば，上の式の行列の行数が$n_s$になります．この式を単純に
$$Y\Theta=F$$
とまとめましょう．ここで$Y\in\mathbb{R}^{n_s\times 2}$は観測値行列，$\Theta\in\mathbb{R^2}$が求めたいパラメータの行列，$F\in\mathbb{R}^{n_s}$が入力値行列になります．$Y$と$F$は既知ですので，この連立方程式を解けば$\Theta$が求まるはずです．
しかしこの方程式は，等式による拘束条件が$n_s$本であるのに対し，未知数は質量$m$と粘性係数$b$の2種類なので，方程式を解くことはできません．その理由は，モデル化が不十分であったり，観測値にノイズが入ってしまうことに起因します（観測ノイズ等がなく，モデル化が完璧であれば，$n_s$本の等式を完璧に満足するパラメータ$m$と$b$が存在します．そんなことは普通有り得ませんが．．．）．

この問題を解決するために，先ほどの式の右辺に誤差行列$\epsilon$を付けてやります．
$$Y\Theta = F + \epsilon$$
この誤差行列$\epsilon=[\epsilon_1, \epsilon_2,\cdots,\epsilon_{n_s}]^T$は，観測時に入るノイズやモデル化誤差などを考慮した値だと思ってください．このノイズは本来分からない値ですので，方程式的には未知数になります．結果的に方程式の未知数は$\epsilon$と求めたいパラメータ$m,\ b$の合計$n_s+2$個で，今回は方程式を解くことができます．

ただ，今回は逆に未知数の方が多くなってしまいました．ここで最小二乗法の考え方が出てきます．
最小二乗法では$m$と$b$を推定するための指針として，「ノイズベクトルの$L_2$ノルム：$\||\epsilon\||_2$を最小にするように$m,\ b$を求める」といったことをしています．できるだけ元の式に合わせるけど，どうしても合わせられない部分は誤差として$\epsilon$で吸収してもらう，といったイメージですね．
つまり，$\hat\Theta$を$\Theta$の最小二乗推定値とすれば，
$$\hat\Theta = {\rm argmin}\ \epsilon^2 = {\rm argmin}\ (Y\Theta - F)^2$$
となります．ここが最小二乗法の美しいところなんですが，$(Y\Theta - F)^2$は$\Theta$に関して下に凸の行列2次関数になるので，この最小値は微分して0になる点で簡単に求められます．実際にやってみると（めんどくさかったら次の数式は飛ばしてください．スカラに分解してコツコツ計算するとできます）

```math
\begin{align}
 \frac{\partial (Y\Theta - F)^2}{\partial \Theta} & = \frac{\partial}{\partial \Theta}(Y\Theta - F)^T (Y\Theta - F)\\
& = \frac{\partial}{\partial \Theta}(\Theta^TY^T - F^T)(Y\Theta - F)\\
& = \frac{\partial}{\partial \Theta}(\Theta^TY^TY\Theta - \Theta^TY^TF - F^TY\Theta + F^TF)\\
& = 2Y^TY\Theta - 2Y^TF = 0
\end{align}
```

よって，今回の最小二乗法（$L2最適化問題$）では最終的に
$$\hat\Theta = (Y^TY)^{-1}Y^TF$$
として得られます．なお，$(Y^TY)^{-1}Y^T$は疑似逆行列と呼ばれる行列で，$Y^{\dagger}$などで表されます．これを使えば，最小二乗推定値は
$$\hat\Theta=Y^{\dagger}F$$
って感じになります．ちなみに$\epsilon$が白色ガウスノイズである場合は，この最小二乗推定値は最尤推定値にもなります．

こんな簡単に推定値が求まるって感動ですね！

これが例えば$\||\epsilon\||_1=\sum_i \|\epsilon_i \|$を最小にする推定問題（${L1}$最適化）だと，すごく計算が大変になります．最近は$L0$最適化ってのも盛んらしいですね．スパース最適化っていうのかな．

ちなみに，この最小二乗法は基本的にオフラインで計算するものなんですが，オンラインでパラメータ推定が可能な「逐次最小二乗法」ってのも存在します．新しいデータが得られるたびに，過去の推定値と新しいデータの重みを考慮しながら，推定値を逐次的に計算します．数式が長ったらしいので書くのはやめますが，ググったらたくさん出てきます．


この辺の機械システムのパラメータ推定は例えば以下の論文とかに結構まとまってます．
>
"A New Closed-Loop Output Error Method for Parameter Identification of Robot Dynamics", IEEE Transactions on Control Systems Technology, vol 21, (2012)

※　余談ですが，最小二乗法を最初に考案したのはルジャンドルと言われています．しかしそれより何年も前にガウスさんが発見していたらしいです．ガウス的には，最適化するなら誤差の二乗だろ．常識でしょ．的なノリでそのときは発表しなかったらしいのですが，あとからルジャンドルが最小二乗法っての見つけた，これすげーよ！と言い出した時に，いや俺の方が先に見つけてたけどね，って感じに喧嘩してたらしいです．


#最小二乗法の課題
こんなに使い勝手のいい最小二乗法でも，やっぱり困ることはたくさんあります．特に困るのは
<b>
・パラメータに対して線形性を必要とする
・微分値とかいちいち計算しないとダメ
</b>
とかですかね．まず，推定したいパラメータがシステムにおいて線形になってないといけないです．材料の曲げとか温度特性のパラメータには$exp$とか$ln$が出てきますが，こいつらの推定はここで述べた手法は使えません．例えば
$$\frac{m}{b}v-\ln(1-mv)=f$$
とかになると，「非線形」最小二乗法を使わないといけません．非線形になると問題が一気に厄介になるので（というか線形が恵まれ過ぎているのですが），微分して0になる点が最尤推定値だ！的な一発計算的な方法はまず存在しません．なので，非線形最小二乗は基本的に再帰計算が必要な勾配法がベースになっています（ニュートン法とか，最急降下法とかですね．レーベンバーグ・マーカート法ってのもあって，収束性が良いと巷ではもっぱらの噂です）．式で書くと
$$\hat\Theta_{k+1}=\hat\Theta_{k} - \alpha \frac{\partial \epsilon^2}{\partial \Theta},\quad \alpha>0$$
こんな感じです．$k$番目のイタレーションの推定値$\hat\Theta$から，次の$k+1$回目の推定値に移動する際に，二乗誤差$\epsilon^2$が小さくなる方へ推定値を変化させるので，各イタレーションで誤差は減っていきます．$\alpha$は勾配定数的なパラメータで，この選び方もいろいろあります．しかし，この方法は根本的に大域収束性を保証できない点や，収束が遅かったりする欠点があります．

また，$\dot v$などの微分値を計算しないといけないのも厄介です．オフラインならまだ何とかなりますが，オンラインだと微分計算は結構めんどくさいです．後退差分でよくね？ってなるかもしれませんが，差分の動作は基本的に高周波数帯においてゲインが非常に高くなるのでノイズの影響がガンガン出てきます．なので別途フィルタリングなどを挟む必要があります．

※　ここでもまた余談ですが，最適化問題を解く際は
1. 近似とか使って，なんとか線形で解けない？ →　線形最小二乗法
2. 無理だった　→　非線形最小二乗法（勾配法）で行くか　→　非線形最小二乗法
3. それも無理だった　→　遺伝アルゴリズム使うか
って流れをよく見かけます．大学の研究室とかで2.を使う前に3.に行くと，だいたい偉い先生方から怒られます．笑
線形最小二乗法はパラメータに対して線形であればいいので，多項式回帰とかにも使えるんですね．なので，結構適用の幅は広いんです．

話を戻しますが，上で述べたように最小二乗法による推定はいろいろと欠点が存在します（というより，欠点を解決する方法はたくさんあるのですが，設計パラメータが多かったり，構成が複雑だったりと，けっこう大変です）．そこで出てくるのが非線形カルマンフィルタによるパラメータ推定です．次回は，拡張カルマンフィルタ（以下EKF:Extended Kalman Filter）と呼ばれる非線形フィルターを用いたオンラインパラメータ推定について説明します．




# ソースコード

とりあえず最小二乗法によるパラメータ推定をMatlabで書いてみたので貼っておきます．
matlabの関数ode45で運動方程式を積分して，その結果にノイズを掛けた値を測定値としてます．
微分は中央差分を使ってるのですが，観測ノイズが大きくなると推定結果がおかしくなります．
本当は微分の前にノイズ処理のフィルターを挟まないとダメです．

```matlab:LS_Estimate.m


% -------------------------------------------------------------------------
% system model: 
%   m * dv/dt + b * v = f
% -------------------------------------------------------------------------


% == simulation ==
% physical parameters
m = 1;
b = 0.1;

% input force
f = @(t)(5 * sin(3 * t));

% dynamics
func = @(t,v)(- b * v / m + f(t) / m);

% initial condition
x0 = 0;

% simulation time
dt = 0.01;
tspan = 0:dt:100;

% integration
option = odeset('RelTol',1e-6, 'AbsTol',1e-8');
[T,V] = ode45(func, tspan, x0, option);


% == Parameter estimation by Least Square method ==

% add noise
noise_v_mean = 0;
noise_v_var = 0.01;
V = V + (noise_v_mean + noise_v_var * randn(length(V), 1));
noise_f_mean = 0;
noise_f_var = 0.01;
F = f(tspan') + (noise_f_mean + noise_f_var * randn(length(tspan'), 1));

% figure plot
figure(1); plot(T,V);grid on;
xlabel('t [s]'); ylabel('v [m/s]');

% central differences
dV = (V(3:end) - V(1:end-2)) / (2 * dt);
V = V(2:end-1);
F = F(2:end-1);

% pseudo inverse calc
Y = [dV, V];
estimated_params = pinv(Y) * F;


fprintf('true value      : m = %3.3f, b = %3.3f\n', m, b);
fprintf('estimated value : m = %3.3f, b = %3.3f\n', ...
    estimated_params(1), estimated_params(2));

```