MATLABで学ぶ カルマンフィルタを用いた振動推定

2022.10.13

はじめに

近年の海外大学のトレンドとして、純粋に振動工学や機械工学を深堀するような研究はされておらず、デジタルツインや状態監視、1DCAE、MBD、メタマテリアルといった研究やワードが目立つようになってきています。これらの分野は、振動工学＋制御工学＋IoT＋システムといったように、ひと昔前だと他分野と見なされた分野を横通しする必要があります。そのため、1人の教授が研究室運営をすることが多い日本の大学では先端の研究ができていないのが現状です。特に振動分野では上記研究テーマで優れた研究がなされていません。

今回紹介する「カルマンフィルタ」は状態変数を推定する手法で、状態変数＝振動とすると振動推定が可能になります。そのため、デジタルツインや状態監視などの分野で適用されています。

カルマンフィルタ自体は制御工学で非常に広く知られていて、制御工学では結構やりつくされた感がある理論です。ただし、制御工学では数自由度のモデルでの適用であり、振動工学で扱うような周波数帯域を扱えなかったのですが、海外大学では振動工学で扱うような周波数帯域への適用を目指しています。

上記のようなトレンドがありますが、振動工学を生業としている日本の方にはあまりなじみがないのではないでしょうか？そもそもトレンドを捉えていないのではないでしょうか？

日本の大学の文献はあまりいいのがないので、本記事では本リンクの論文を参考にしたいと思います。カルマンフィルタについては下記の著書を参考にしています。下記著書にはカルマンフィルタなどのMATLABプログラムが掲載されています。

 

なお、カルマンフィルタは状態空間モデルを用いて状態変数を推定する手法です。ただし状態変数モデルは離散化する必要があります。状態空間モデルの離散化については下記を参照ください。


https://mech-eng-uni.com/matlab%e3%81%a7%e5%ad%a6%e3%81%b6-%e7%8a%b6%e6%85%8b%e7%a9%ba%e9%96%93%e3%83%a2%e3%83%87%e3%83%ab%e3%81%ae%e9%9b%a2%e6%95%a3%e5%8c%96/

状態空間モデル（N自由度のバネ-マス-ダンパモデル）

前回の記事と重複するので、簡単におさらい部分だけ記載します。図1のようなN自由度の運動方程式は式(1)でしたね。なお、細かい文字式についての説明は前回の記事を参照ください。

図1　バネマスモデル（N自由度）

$$
[M][\ddot{y} (t)] + [C][\dot{y} (t)] + [K][y(t)]=[u(t)] \tag{1}
$$

式(1)の状態方程式は式(3)になり、出力方程式は式(4)になりましたね。

$$
\left( \begin{array}{c}
[ \dot{x_1}] \\
[ \dot{x_2}]
\end{array} \right) =
\left( \begin{array}{cc}
[0]  & [I] \\
-[M]^{-1}[K] & -[M]^{-1}[C]
\end{array} \right)
\left( \begin{array}{c}
[x_1] \\
[x_2] [u]
\end{array} \right) +
\left( \begin{array}{c}
[0] \\
[M]^{-1}
\end{array} \right)  \tag{3}
$$

$$
[y]=
\left( \begin{array}{cc}
[I] & [0]
\end{array} \right)
\left( \begin{array}{c}
[x_1] \\
[x_2]
\end{array} \right) \tag{4}
$$

一般化すると式(5)のようになりましたね。

\begin{eqnarray}
\left\{
\begin{array}{l}
[\dot{x} (t)] = [A][x(t)]+[B][u(t)] \\
[y(t)] = [C] [x(t)]
\end{array}
\right.
\tag{5}
\end{eqnarray}

状態空間モデル（状態方程式と出力方程式）は連続時間系なので、カルマンフィルタを適用するためには離散化する必要があります。

離散化した状態方程式と出力方程式は式(6)になります。

\begin{eqnarray}
\left\{
\begin{array}{l}
[x(k+1)] = [A_d][x(k)]+[B_d][u(k)] \\
[y(k+1)] = [C_d] [x(k)]
\end{array}
\right.
\tag{6}
\end{eqnarray}

離散化手法は複数ありますが、本プログラムではZOHという手法を使っています。ZOHについてはこちらを参照ください。

前回の解説で記載するの忘れておりましたが、C=Cdとなります。

カルマンフィルタ

状態空間モデルでは状態変数x[k]、出力変数y[k]という表記・表現を使いますが、カルマンフィルタでは事後状態推定値\(\hat{x}[k]\)、観測ベクトルy[k]という表記・表現を使います。

また、状態空間モデルでは状態変数x[k]や出力変数y[k]を求めるのに対し、カルマンフィルタでは観測ベクトルy[k]を用いて、事後状態推定値\(\hat{x}[k]\)を推定します。

カルマンフィルタを適用した式(6)の状態方程式は、状態変数x[k+1]およびx[k]が事前状態変数推定値\(\hat{x}^{-}[k+1]\)と事後状態推定値\(\hat{x}[k]\)に置き換わり、式(7)で表されます。

$$
[\hat{x}^{-}[k+1]]=[A_d] [\hat{x}[k]] + [B_d] [u[k]]  \tag{7}
$$

また、カルマンゲインg[k+1]と事前誤差共分散行列\([P^{-}[k+1]]\)を式(8)と式(9)で算出できます。

$$
[g[k+1]]=[P^{-}[k+1]][C_d]^T([C_d] [P^{-}[k+1]] [C_d]^T + [R])^{-1}  \tag{8}
$$

$$
[P^{-}[k+1]]=[A_d] [P[k]] [A_d]^T + [Q]  \tag{9}
$$

カルマンゲインg[k]より、事後状態推定値\(\hat{x}[k+1]\)を求めることが式(10)で求めることができます。

$$
[\hat{x}[k+1]]=[\hat{x}^{-}[k+1]] + [g[k+1]] ([y[k+1]]-[C_d][\hat{x}^{-}[k+1]]) \tag{10}
$$

最後に、事後誤差共分散行列P[k]を式(11)で求めます。

$$
[P[k+1]]=[P^{-}[k+1]]+[g[k+1]][C_d][P^{-}[k+1]]  \tag{11}
$$

こちらの参考文献では、観測誤差共分散行列Rとモデル化誤差共分散行列Qは、任意定数rとqを用いて記のように定式化（モデル化）しているようです。

$$
[R]=r[I]　　,　[Q]=q[I]　  \tag{10}
$$

 

MATLABで解くカルマンフィルタでの振動推定

では、下記の例題を解いてみましょう。

図1　バネマスモデル（N自由度）

  • 自由度N=10
  • k=10^5
  • c=1
  • m=1
  • サンプリング周波数fs=400（dt=1/fs）
  • 初期値のx：m1の変位を1、m2～m10の変位を0
  • y[k]：m5とm10の変位
  • 求めたい状態変数：変位1

全自由度の変位と速度が求まりますが、ここではm1の変位の結果だけを紹介します。MATLABの「ode45」が正解(真値)として、「ode45で求めたm5とm10の振動変位」と「モデル（10自由度モデル）」を用いて、m1の振動を推定できるか検証した結果が図2です。

図2　カルマンフィルタでの振動推定結果

本例題は変位加振をしています。そのため、初期の変位状態を未知として振動推定するカルマンフィルタでは、推定し始めの0～0.03s間の振動推定精度は悪いですが、それ以外の時間では振動が推定できていることが確認できますね。

本例題では「真値を作成するときに使用したモデル」と「カルマンフィルタでの振動推定をするときに使用したモデル」が同じモデルを使用しています。

そのため、カルマンフィルタでの振動推定時のモデル化誤差の定数qは0に近い値を使用すればよいことになります。また、観測ベクトルにはノイズも含まれていないのでrも0に近い値を使用すればよいことになります。

実際にカルマンフィルタでの振動推定をする場合は、観測ベクトルにはノイズが含まれており、また実機とモデルにも誤差があると考えられますので、qやrを適切に設定する必要があります。

MATLABプログラム

実行ファイル

clear all;clc;close all

m_vec=ones(1,10);
k_vec=ones(1,10)*10^5;
[M]=eval_Mmatrix(m_vec); %質量行列
[K]=eval_Kmatrix(k_vec); %剛性行列
C=K*10^-5;


% % % 出力点 
observe_point_id=[5 10];
observe_pointN=length(observe_point_id);
Sd=zeros(observe_pointN,size(M,2));
for ii1=1:observe_pointN
Sd(ii1,observe_point_id(ii1))=1;
end

% % 状態方程式（連続時間系）
% % dx/dt =Ac x(t) + Bc p(t)
Ac=[zeros(size(M)) eye(size(M))
    -inv(M)*K -inv(M)*C];
Bc=[zeros(size(M))
    -inv(M)];

Cc=[Sd zeros(size(Sd))];
Dc=0;

x0 = zeros(size(M,1)*2,1);
x0(1) =1;
t0 = 0;
dt=1/400;
tf = 10;
time=t0:dt:tf;

[T,Y] = ode45(@state_equation, time, x0); %理論値
figure
subplot(211)
plot(T,Y(:,1:10))
xlabel('Time [s]')
ylabel('Displacement [m]')
subplot(212)
y_k=Cc*Y.';
plot(T,y_k)
xlabel('Time [s]')
ylabel('Acc. [m/s^2]')





% % % % % 状態方程式（離散時間系）
AcBc00=[Ac Bc
    zeros(size(Bc,2),size(Ac,2)) zeros(size(Bc,2),size(Bc,2))];
AB00=expm(AcBc00*dt);
OA=AB00(1:size(Ac,1),1:size(Ac,2));
OB=AB00(1:size(Ac,1),size(Ac,2)+1:end);
OC=Cc;

% P_k=zeros(size(OA));
P_k=eye(size(OA));
Qa=eye(size(OA))*10^-30; %モデル化誤差共分散行列
Rk1=eye(observe_pointN)*10^-30; %観測誤差共分散行列
x_hat_k=zeros(size(M,1)*2,1);
u_k=zeros(size(M,1),1);
x=zeros(size(OA,1),length(T));
for ii1=1:length(T)
[x_hat_k,P_k] = Kalman_filter(y_k(:,ii1),u_k,OA,OB,OC,Rk1,x_hat_k,P_k,Qa);
x(:,ii1)=x_hat_k;
end

figure
plot(T,Y(:,1),'k','linewidth',2)
hold on
plot(time,x(1,:),'r--','linewidth',1)
legend('真値(m1の変位)','m5とm10からカルマンフィルタで推定したm1')
xlabel('Time [s]')
ylabel('Displacement [m]')

 

functionファイル

function [x_hat_k,P_k] = Kalman_filter(y_k,u_k,OA,OB,OC,Rk1,x_hat_k_1,P_k_1,Qa)
% % % 島田著「外乱オブザーバ」P.191参照

% % 事前推定
x_hat_minus_k = OA*x_hat_k_1 + OB*u_k;   % 事前状態推定値
P_minus_k = OA*P_k_1*OA' + Qa; % 事前共分散行列

% % カルマンゲイン
G_k = P_minus_k*OC'*inv(OC*P_minus_k*OC'+Rk1);

% % 事後推定
x_hat_k = x_hat_minus_k +G_k*(y_k-OC*x_hat_minus_k); % 状態推定値
P_k = P_minus_k - G_k*OC * P_minus_k; % 事後共分散行列

end

function [M]=eval_Mmatrix(m_vec)

M=diag(m_vec);

end

function [K]=eval_Kmatrix(k_vec)

K=zeros(length(k_vec));
for ii1=1:length(k_vec)
    if ii1==1
        K(ii1,ii1)=k_vec(ii1);
    else
        K(ii1-1:ii1,ii1-1:ii1)=K(ii1-1:ii1,ii1-1:ii1)+[1 -1;-1 1]*k_vec(ii1);
    end
end

end

function dx=state_equation(t,x)


m_vec=ones(1,10);
k_vec=ones(1,10)*10^5;
[M]=eval_Mmatrix(m_vec); %質量行列
[K]=eval_Kmatrix(k_vec); %剛性行列
C=K*10^-5;

% [V,D]=eig(K,M); %固有値D、固有ベクトルV
% wn=sqrt(diag(D)); %固有角振動数
% fn=wn/(2*pi); %固有振動数


% % 状態方程式
% % dx/dt =Ac x(t) + Bc p(t)
Ac=[zeros(size(M)) eye(size(M))
    -inv(M)*K -inv(M)*C];
Bc=[zeros(size(M))
    -inv(M)];

% Cc=[-Sa*inv(M)*K -Sa*inv(M)*C];
% Dc=Sa*inv(M);
p=zeros(size(M,1),1);
% size(p)
% size(Bc)
% size(Bc*p)
% 
% size(Ac)
% size(Ac*x)
dx=Ac*x+Bc*p;
end

 

 

コメント