はじめに
書かれていること
この記事では具体例を示しながらカルマンフィルタとは何か、何が出来るのかをついて解説します。カルマンフィルタについては、様々な方が既に解説記事・書籍を投稿しておりますが、初学者（特に組み込み技術者）にとって「じゃあ具体的にどう解釈すればよいの？どう実装すればいいの？」といったところが弱い気がして、もったいないと感じたため、その辺を補完する記事が書ければと思っています。
さて、この記事は下記の順で解説します。
- カルマンフィルタとは何か
- なぜカルマンフィルタを使うのか
- 具体的な実装例
- 応用例
自分のモチベーションとしては、最近カルマンフィルタを勉強して、「なんて便利な道具なんだ！」と感じたため、それを共有する目的で記載しております。すこしでも「便利だなあ」と感じていただければ幸いです。また、この記事は、組み込み技術者としての私の視点から見た解釈で記載しております。もし内容に間違いがございましたらご指摘いただければ幸いです。
この記事は、学術的に正確な事実を記載した記事ではなく、あくまで工学的な嬉しさにフォーカスした記事となっています。正確なカルマンフィルタの定義や適応例を知りたい方はぜひ関連書籍をご覧ください。
想定する知識
この記事では深くカルマンフィルタの原理や考え方には踏み込みません。なぜならば現場で適用するだけならば、物理式を立てて簡単な行列計算さえできればＯＫなためです。そのため、読むために必要な知識としては、下記の知識があれば問題ありません。
- 高校物理の知識
- 簡単な行列計算の知識
すこし統計的な話も出てきますが、本筋には関係ないので読み飛ばしても問題ありません）。ただし、関連書籍に書いてあるカルマンフィルタの原理を理解したい！となった場合、確率統計の知識が必要となります。原理を知ることで応用範囲を増えると思うので、さらに知りたければ頑張って理解してみることをオススメします。（というか、私もまだ勉強中です）
カルマンフィルタとは？
一言でいうと、カルマンフィルタは「モデル」を活用したフィルタのことです。ここでいう「モデル」とはバネ・マス・ダンパ系のような、いわゆる高校～大学で習うような物理モデルのことです。この「モデル」をフル活用することで、ノイズが多い環境の中でも、高い精度で真の値を取り出すことが出来ます。
カルマンフィルタでしたいこと
例えば以下の物理系を考えてみましょう。
図１：理想的なバネ・マス系
図１は理想的なバネ・マス系を示しています。理想的なというのは「ノイズがない」という意味です。
ちなみに、ノイズにも２種類あります。
- 観測ノイズ
- 観測センサに乗るノイズ(e.g. センサ計測精度の限界、電磁放射ノイズetc)
- システムノイズ
- 物理系（システム）に乗るノイズ(e.g. 地下鉄の振動、地震、モデル化の際に無視した微小要素etc)
ここでは、観測ノイズ・システムノイズが両方ない場合を「理想的な」バネ・マス系としています。
一方、現実の物理系ではノイズが存在します。
図２：ノイズ有のバネ・マス系
さて、あなたは観測値からバネ・マス系の重りの正確な位置を推測したいとします。重りには位置センサが付いています。この時、どうすれよいでしょうか。よく使われる方法として、平均値フィルタや、ローパスフィルタがあります。
しかし、我々は重りの位置が運動方程式に従うことを知っています。この情報を使って賢く推測できないでしょうか？
図３：推測イメージ
以上がカルマンフィルタの「やりたいこと」になります。なお、当然のことながら、カルマンフィルタはバネ・マス系以外にも様々なシステムに適用可能です。一例を下記適用例に記載してあります。
では、具体的にカルマンフィルタの作り方を見ていきます。カルマンフィルタ作成は下記の２ステップで行います。
- システムの物理モデルから状態空間モデルを作成
- 状態空間モデルからカルマンフィルタを作成
カルマンフィルタを使った解決法
状態空間モデルを作成
まず、物理モデルを状態空間モデルに式変形します。あらゆる物理モデルは状態空間モデルに式変形可能です。これにより、あらゆるシステムを一括して議論することが可能になります。下記に状態空間モデルを示します。
\begin{align}
\boldsymbol{x}(t+1)&=\boldsymbol{Ax}(t)+\boldsymbol{b}v(t) &:状態方程式\\
y(t)&=\boldsymbol{c}^T \boldsymbol{x}(t)+\omega(t) &:観測方程式
\end{align}
式１：状態空間モデル
この式は、時刻$t \rightarrow t+1$の時のシステムの状態変化を示しており、$\boldsymbol{x}$を状態ベクトル、$\boldsymbol{A}$はシステム行列、$\boldsymbol{b}$はシステムノイズの影響を表すn次元ベクトル、$v$はシステムノイズ、$y$は観測値、$c$は観測係数ベクトル、$\omega$は観測ノイズを示しています。実際にバネ・マス系を状態空間モデルに変換してみます。
バネ・マス系の運動方程式は外力$f$も入れて
m \frac{d^2}{dt^2}x+kx=f
です。ここで$x = x_1 $と置くと、
m \frac{d^2}{dt^2}x_1+kx_1=f
と記述できます（当たり前ですが）。さらに、$\frac{d}{dt}x_1=x_2$と置くと
m \frac{d}{dt}x_2+kx_1=f
と変形できます。
これを行列で表すと下記のように記述できます。
\frac{d}{dt}
\begin{bmatrix}
x_1\\
x_2
\end{bmatrix}
=
\begin{bmatrix}
0 & 1\\
-\frac{k}{m} & 0
\end{bmatrix}
\begin{bmatrix}
x_1\\
x_2
\end{bmatrix}
+
\begin{bmatrix}
0\\
f
\end{bmatrix}
ここで
\begin{align}
\boldsymbol{x} &=
\begin{bmatrix}
x_1\\
x_2
\end{bmatrix}\\
\boldsymbol{v} &=
\begin{bmatrix}
0\\
f
\end{bmatrix}\\
\boldsymbol{b}&=
\begin{bmatrix}
1 & 0\\
0 & 1
\end{bmatrix}
(= \boldsymbol{I}(単位行列))
\end{align}
と置くと
\frac{d}{dt}
\boldsymbol{x} =
\begin{bmatrix}
0 & 1\\
-\frac{k}{m} & 0
\end{bmatrix}
\boldsymbol{x}
+
\boldsymbol{b}v
と書け、さらに、タイムステップを$\Delta t$としたとき、$t \rightarrow t+1$の間で
\begin{align}
\boldsymbol{x}(t+1)&=\boldsymbol{x}(t)+\frac{d\boldsymbol{x}(t)}{dt}dt\\
&=\boldsymbol{x}(t)+\frac{\Delta \boldsymbol{x}(t)}{\Delta t}\Delta t
\end{align}
の関係があることから
\begin{align}
\boldsymbol{x}(t+1)
&=\boldsymbol{x}(t)+\Bigl( \begin{bmatrix}
0 & 1\\
-\frac{k}{m} & 0
\end{bmatrix} \boldsymbol{x}(t) \Bigr)\Delta t \\
&=\boldsymbol{Ix}(t)+\Bigl( \begin{bmatrix}
0 & 1\\
-\frac{k}{m} & 0
\end{bmatrix} \boldsymbol{x}(t) \Bigr)\Delta t\\
&=\begin{bmatrix}
1 & \Delta t\\
-\frac{k}{m}\Delta t & 1
\end{bmatrix} \boldsymbol{x}(t)\\
&=\boldsymbol{Ax}(t)
\end{align}
となります。
さらに、観測方程式は、位置センサでマスの位置のみを観測できるとした場合
y = x_1\\
=\begin{bmatrix}
1\\
0
\end{bmatrix}^T
\begin{bmatrix}
x_1\\
x_2
\end{bmatrix}\\
=\boldsymbol{c}^T\boldsymbol{x}(t)
と変形できます。加えて、観測ノイズ$\omega(t)$が乗ることを考えると
y
=\boldsymbol{c}^T\boldsymbol{x}(t)+\omega (t)
となるため、最終的に状態空間モデルに変形した結果は下記の通りとなります。
\begin{align}
\boldsymbol{x}(t+1)&=\boldsymbol{Ax}(t)+\boldsymbol{b}v(t)\\
y(t)&=\boldsymbol{c}^T \boldsymbol{x}(t)+\omega(t) \\
\boldsymbol{A} &=
\begin{bmatrix}
1 & \Delta t\\
-\frac{k}{m}\Delta t & 1
\end{bmatrix}\\
\boldsymbol{b} &= \boldsymbol{I}\\
\boldsymbol{c} &=
\begin{bmatrix}
1 \\
0
\end{bmatrix}\\
\end{align}
この結果MATLABを用いて確かめてみましょう。プログラムは下記の通り作成しました。ここで、各$k$、$m$等の各係数や観測ノイズ$\omega$の標準偏差等は適当に置きました。
clear;
% setting
delta_t = 0.0001; %[s]
k = 2; %[N/m]
m = 0.1; %[kg]
x1 = 0.1; %[m]
x2 = 0.0; %[m/s]
std_distr = 0.005; % 観測ノイズの正規分布の標準偏差[-]
start_time = 0;
finish_time = 5;
step_num = (finish_time - start_time)/delta_t;
result = zeros(2,step_num); %[s, m]
A = [1, delta_t;
-k*delta_t/m, 1 ];
x = [x1; x2];
c = [1; 0];
omega_w = 0.005^2;
%calculate
for index = 0:step_num;
time = index * delta_t;
x = A*x;
y = transpose(c)*x + (std_distr*randn);
result(2,index+1) = time;
result(1,index+1) = y;
end
plot(result(2,:),result(1,:),"");
xlabel("time[sec]");
ylabel("position[m]");
grid on
hold on
この結果は下記の通りです。
上記グラフの通り、バネ・マス系が状態空間モデルに変換できたことが分かります。
カルマンフィルタの作成
カルマンフィルタは下記の式の順で求めていきます。
- 予測ステップ
\begin{align}
\boldsymbol{\hat{x}^-}(t) &= \boldsymbol{A} \boldsymbol{\hat{x}}(t)\\
\boldsymbol{P^-}(t) &= \boldsymbol{AP}(t-1)\boldsymbol{A}^T + \sigma_v^2 \boldsymbol{b}\boldsymbol{b}^T
\end{align}
- フィルタリングステップ
\begin{align}
\boldsymbol{g}(t) &= \frac{\boldsymbol{P}^-(t) \boldsymbol{c}}{\boldsymbol{c}^T\boldsymbol{P}^-(t)\boldsymbol{c}+\sigma_\omega ^2}\\
\boldsymbol{\hat{x}}(t) &= \boldsymbol{\hat{x}^-} + \boldsymbol{g}(t)(y(t)-\boldsymbol{c}^T \boldsymbol{\hat{x}^-}(t))\\
\boldsymbol{P}(t) &= (\boldsymbol{I}-\boldsymbol{g}(t)\boldsymbol{c}^T)\boldsymbol{P}^-(t)
\end{align}
これをMATLABで実装したソースコードは下記の通りとなります。
clear;
% setting
delta_t = 0.0001; %[s]
k = 2; %[N/m]
m = 0.1; %[kg]
x1 = 0.1; %[m]
x2 = 0.0; %[m/s]
I = eye(2);
b = I;
P = I; % 単位行列を初期値とする（カルマンフィルタ内で更新していくためOK）
std_distr = 0.005; % 観測ノイズの正規分布の標準偏差[-]
start_time = 0;
finish_time = 5;
step_num = (finish_time - start_time)/delta_t;
result = zeros(5,step_num); %結果保存用領域[s, true m, observed m, estimated m]
A = [1, delta_t;
-k*delta_t/m, 1 ];
x = [x1; x2];
c = [1; 0];
x_prev = x;
x_hat = x_prev;
omega_w = 0.005^2;
%calculate
for index = 0:step_num;
time = index * delta_t;
%% real physical system
x = A*x_prev;
y = transpose(c)*x + (std_distr*randn);
%% kalman filter calc in controller
% prediction step
x_hat_min = A * x_hat;
P_min = A*P*transpose(A) + b * transpose(b) * 0^2;
% filtering step
g = P_min*c/(transpose(c)*P_min*c + std_distr^2);
x_hat = x_hat_min + g * (y - transpose(c)*x_hat_min);
P = (I - g*transpose(c))*P_min;
% next step iteration(k<=k-1)
x_prev = x;
% logging
result(4,index+1) = transpose(c)*x_hat; % estimated value
result(3,index+1) = y; % observed value
result(2,index+1) = transpose(c)*x; % true value
result(1,index+1) = time;
end
% plot result
plot(result(1,:),result(3,:),'g-');
xlabel("time[sec]");
ylabel("position[m]");
grid on
hold on
plot(result(1,:),result(4,:),'r-x');
plot(result(1,:),result(2,:),'b-', 'LineWidth',1);
legend('observed value[m]','estimated value[m]','true value[m]');
結果は下記の通りとなりました。
この結果より、観測値では大きくノイズが乗っているのにも関わらず、推測値はほぼ真値と一致していることからフィルタリングが出来ていることが分かります。