最小二乗法でシステム同定やってみた

はじめに

私が制御工学を学び始めたとき、「これを学んでいけばモータを自由に制御できるようになるのか！」と思い、古典制御、現代制御と勉強を続けましたが、「これってモデルがある前提で説明してくるけど、そもそもモデルってどうやって求めるの？」という疑問が湧きました。制御工学を学んでいる人でもこういった疑問を持つ人は多いのではと思ったので、今回は実際に水平1軸アームシステムを対象に、システム同定を行い、モデルの妥当性を簡単に評価したいと思います。

目次

1. システム同定とは
2. 水平1軸アームシステムのモデル
3. 同定入力の選定
4. システム同定の手法
5. システム同定
6. モデルの妥当性評価

1. システム同定とは

制御工学を勉強してPID制御やら最適レギュレータやら様々なコントローラ・オブザーバの設計方法を知ると思います。しかしそれらを設計するとき、必ず必要になるのが数学モデルです。教科書や授業ではさらっと「モデルのパラメータはこうなっています」、と流されることが多いと思います。質量や長さなど測定しやすいパラメータならともかく、バネ定数や粘性摩擦係数、慣性モーメント、摩擦力など、データシートに書いてなければどうやって測定するの？というものは世の中多いと思います。私も以前ロボットを作ったとき、こんな便利なコントローラ設計方法があるなら使えるじゃん！と思い、設計しようにもモデルはないし、適当にパラメータを測定してモデルを作っても、実際のシステムとシミュレーションが全然一致しないしで散々でした。

前述した通り、制御理論を使うためには対象とするシステムの特性を表現できる数学モデルが必要となります。最初に思いつくモデリング方法としては、システムを支配する物理法則に基づいてモデリングを行う方法だと思います。しかし、物理法則を用いて導出することが困難だったり、システムが複雑だったりする場合はモデリングができないです。そういったときには、対象とするシステムに適当な入力を与え、その出力を観測することで、入出力間の関係を表す数学モデルを決定することができます。これをシステム同定といいます。

2. 水平1軸アームシステムのモデル

水平面を回転する1軸アームシステムの運動方程式は次のように表されます。このとき${\theta(t)}$をアームの角度変位${\mathrm{[rad]}}$、${J}$を慣性モーメント${\mathrm{[kg\cdot m^2]}}$、${C}$を粘性摩擦係数${\mathrm{[N\cdot m/(m/s)]}}$、${\tau(t)}$を入力トルク${\mathrm{[N\cdot m]}}$、${\tau_d}$を動摩擦トルク${\mathrm{[N\cdot m]}}$とします。

J\ddot{\theta}(t)+C\dot{\theta}(t)=\tau(t)-{\tau}_{d}(t)\mathrm{sgn}(\dot{\theta}(t)) \tag{2.1}

今回、実際にモータに入力しているのは電圧${V(t)}$ですので、式(2.1)を電圧入力に変更し、次のように整理します。このとき${i(t)}$を電流${\mathrm{[A]}}$、${K_T}$をトルク定数${\mathrm{[Nm/A]}}$、${R}$を内部抵抗${\mathrm{[\Omega]}}$とします。

\begin{align}
\ddot{\theta}(t) &= -\frac{C}{J}\dot{\theta}(t) - \frac{\tau_{d}}{J} \mathrm{sgn} (\dot{\theta}(t)) + \frac{\tau(t)}{J}\\
&= -\frac{C}{J}\dot{\theta}(t) - \frac{\tau_{d}}{J} \mathrm{sgn} (\dot{\theta}(t)) + \frac{K_T}{J} i(t)\\
&= -\frac{C}{J}\dot{\theta}(t) - \frac{\tau_{d}}{J} \mathrm{sgn} (\dot{\theta}(t)) + \frac{K_T}{JR} V(t) \tag{2.2}
\end{align}

このとき${\ddot{\theta}(t)}$、${\dot{\theta}(t)}$、${V(t)}$の各係数をそれぞれ${\alpha}$、${\beta}$、${\gamma}$とすると、式(2.2)は次のように表されます。

\begin{align}
\ddot{\theta}(t)=-\alpha \dot{\theta}(t)-\beta \mathrm{sgn} (\dot{\theta}(t))+\gamma V(t) \tag{2.3}
\end{align}

今回はこの各係数を最小二乗法に基づく方法と予測誤差法(PEM)を用いて同定してみます。

図2.1. 水平面を回転するアームシステム

3. 同定入力の選定

早速システム同定するぞ！といきたいところですが、まずシステム同定に用いる入力信号の選定を行います。システム同定の精度は同定入力に大きく影響し、システムの周波数特性や振幅特性を適切に考慮する必要があります。

3.1. 同定入力の周波数特性

システムの周波数特性を表現するためには、同定入力はシステムのもつすべてのモードを励起してくれるような入力が理想的です。つまり入力信号がすべての周波数成分を含む必要があります。本来はPE性とかの話を交えながら説明するのがいいんですけど、今回は割愛します。(説明できるくらい理解出来たら書くかも)
入力信号の周波数成分が大事ってのはわかったけど、上記の条件を満たす理想的な信号って何なの？ってなりますが、その信号が白色雑音になります。白色雑音はすべての周波数成分を含んでおり、システム同定においては理想的な入力信号です。しかし、理想的な白色雑音は作り出せないので、後述するM系列信号や、表現したい周波数帯域付近の正弦波信号などを入力します。

3.2. 同定入力の振幅特性

同定入力の振幅も、前述した周波数特性同様にシステム同定においてはとても重要です。
例えば同定入力の振幅が小さいと、静止摩擦力などに邪魔されシステムが動作しない不感帯の影響を受けやすくなります。逆に振幅が大きすぎると、出力が飽和してしまう場合もあります。このように同定入力の振幅はシステムの特性に大きく影響するため、適切に決定する必要があります。
「じゃあシステムが同定実験で壊れても嫌だし、静止摩擦力で止まらないくらいの小さい信号でもいいかな」って昔の私は思っていたのですが、出力信号の測定値の雑音や、出力を信号処理する際のことを考えると振幅は可能な限り大きくすると良いです。

3.3. M系列信号

3.1節でも少し触れたM系列信号について説明します。前述した通り、理想だけでいえば白色雑音を同定入力として使用したいのですが、白色雑音は物理的に実現できません。そこで疑似的な不規則信号を生成します。線形システム同定の場合、2値信号で十分なので、よく疑似白色2値信号が利用されます。その疑似白色2値信号の中でも、M系列信号はよく知られ、様々なシステム同定で利用されています。

周期${N}$のM系列信号は次のように表されます。このとき${x_k}$はM系列信号、${a_i}$ ${(i=1,2,\dots,n)}$はシフトレジスタの係数、${n}$はシフトレジスタの個数となります。シフトレジスタの係数${a_i}$は0か1の値をとります。また${\oplus}$は直和、つまりXORです(1を真、0を偽に対応させた場合)。

x_k=a_1 x_{k-1} \oplus a_2 x_{k-2} \oplus \dots \oplus a_n x_{k-n} \tag{3.1}

今回は${n=8}$、フィードバックタップの位置を7段目から2段目としました。
図3.1に生成したM系列信号、3.2にM系列信号をFFTした結果とM系列信号の自己相関関数を示します。図のとおり、FFTの結果を見るとM系列信号は広帯域にわたる周波数成分を持っていることがわかります。自己相関関数の値は、ラグが93、186のときに大きくなっています。つまりこのM系列信号は周期93の周期関数となります。今回はこれを同定信号とします。

図3.1. M系列信号

図3.2. M系列信号の周波数特性(上)と自己相関関数(下)

4. システム同定の手法

4.1. 最小二乗法によるシステム同定

最小二乗法によるシステム同定について簡単に説明します。まず式(2.3)より、少し式を整理します。

\begin{align}
\ddot{\theta}(t)&=-\alpha \dot{\theta}(t)-\beta \mathrm{sgn} (\dot{\theta}(t))+\gamma V(t)
\end{align}

\ddot{\theta}(t) =
    \begin{bmatrix}
    -\dot{\theta}(t) & -\mathrm{sgn} (\dot{\theta}(t)) & V(t)
    \end{bmatrix}
    \begin{bmatrix}
    \alpha\\
    \beta\\
    \gamma\\
    \end{bmatrix}

N(t)=\boldsymbol{M}(t)\boldsymbol{p} \tag{4.1}

一定周期でサンプリングした場合、式(4.1)より${\boldsymbol{p}}$は次のように求めることができます。

\begin{bmatrix}
\ddot{\theta}(0)\\
\ddot{\theta}(1)\\
\vdots\\
\ddot{\theta}(k)
\end{bmatrix}
=
\begin{bmatrix}
-\dot{\theta}(0) & -\mathrm{sgn} (\dot{\theta}(0)) & V(0)\\
-\dot{\theta}(1) & -\mathrm{sgn} (\dot{\theta}(1) & V(1)\\
\vdots & \vdots & \vdots\\
-\dot{\theta}(k) & -\mathrm{sgn} (\dot{\theta}(k) & V(k)\\
\end{bmatrix}

\begin{bmatrix}
\alpha\\
\beta\\
\gamma\\
\end{bmatrix}

\begin{align}
\boldsymbol{N}_s &= \boldsymbol{M}_s \boldsymbol{p}\\
\boldsymbol{p} &= ({\boldsymbol{M}_s}^{T} {\boldsymbol{M}_s})^{-1} {\boldsymbol{M}_s}^{T} \boldsymbol{N}_s\\
\boldsymbol{p} &= {\boldsymbol{M}_s}^{\dagger} \boldsymbol{N}_s \tag{4.2}
\end{align}

ただし${{\boldsymbol{M}_s}^{\dagger}}$を${{\boldsymbol{M}_s}}$の疑似逆行列とします。こんな簡単な式でシステム同定できるなんてすごいですよね～。
また「これのどこが最小二乗法なんだ？」と思われる方もいると思うのですが、そのあたりが気になる人は参考文献を見てください(丸投げ)。

4.2. 予測誤差法(PEM)

今回は式(2.3)をモデルとしているので、このモデルの各係数を推定できればよいです。そこでシステム同定をパラメータ推定問題に帰着させます。パラメータ推定のための評価関数を次に示します。ただし${N}$をサンプル数、${y}$を出力の観測値、${\hat{y}}$を出力の推定値、${\varepsilon(k,\boldsymbol{p})}$を予測誤差とします。

\begin{align}
J_N(\boldsymbol{p}) &= \frac{1}{N} \sum_{k=1}^N \varepsilon^2(k,\boldsymbol{p}) \tag{4.3}\\
\varepsilon(k,\boldsymbol{p}) &= y(k) - \hat{y}(k|\boldsymbol{p}) \tag{4.4}
\end{align}

システム同定をするには、式(4.3)の評価関数を最小化するパラメータベクトル${\boldsymbol{p}}$を推定すること、つまり${\hat{\boldsymbol{\theta}}(N)}$を決定できます。

\hat{\boldsymbol{\theta}}(N) = \mathrm{arg}\underset{\boldsymbol{\theta}}{\min} J_N(\boldsymbol{\theta}) \tag{4.5}

このように予測誤差から構成される評価関数${J_N(\boldsymbol{\theta})}$を最小にするように推定値を計算するパラメータ推定法を、予測誤差法(PEM : Parameter Estimation Method)と呼びます。

4.3. 最小二乗法と予測誤差法の違い

4.1、4.2節では最小二乗法に基づくシステム同定と予測誤差法について説明しましたが、予測誤差法も予測誤差の二乗が評価関数であり、その評価関数を最小化しようとしているので、最小二乗法と同じことをやっています。私も最初気づいたとき、「これ結局どっちも最小二乗法ってことは同じなのか？」と思っていたので調べてみると細かい違いがあるみたいです。
4.1節で説明した最小二乗法は、解析的にパラメータを求める手法であり、主に線形システムに適用されることが多いです。一方、予測誤差法は反復的に最適化を行う手法であり、非線形システムや動的システム、MIMOシステムなど、より多様なシステムに対して適用が可能です。これだけ説明すると予測誤差法の方が良さそうですが、計算コストを考えると、4.1節の手法の方が高速です。結局、同定するシステムとか状況によって手法を使い分ける方がいいと思います。

5. システム同定

5.1. 実験装置

今回、作成した実験装置を図5.1に示します。基本的には3Dプリンタを利用して製作しました。今回は水平1軸アームシステムということで水平に組んでいますが、今後、垂直でもやりたくなると思い、組み換えできるように設計しました。使用した主要部品を表5.1に示します。部品は基本的に部屋に落ちてた余りものを使用しました。図5.1のようにモータとエンコーダは歯車でつながっており、減速比は${1:1}$になっています。またエンコーダは4逓倍で使用したので分解能としては${2.618 \times 10^-3 \mathrm{[rad/pulse]}}$となっています。モータドライバはIR2302を使ったただのモータドライバです。電源電圧は${12 \mathrm{[V]}}$、入力するPWMの周波数は${20 \mathrm{[kHz]}}$としました。

表5.1. 実験装置の部品詳細

項目	詳細
DCモータ	DME34B37G30A (12V)
エンコーダ	E6B2-CWZ6C (600P/R)
マイコン	ESP32-DevKitC ESP32-WROOM-32開発ボード 4MB

図5.1. 作成した実験装置

5.2. 実験方法

今回は実験装置にM系列信号を入力します。M系列信号の仕様を表5.2に示します。シフト周波数が微妙な値になってるのは事前に試行錯誤したところこのくらいがちょうどよかったのでこんなことになっています(きりのいい値にしとけばよかった)。
サンプリング周波数は${200 \mathrm{[Hz]}}$とし、エンコーダでアーム角度を計測します。角速度、角加速度はオフラインで中心差分近似を用いて計算します。角速度、角加速度では差分近似によるチャタリングが多く発生したため、カットオフ周波数${35 \mathrm{[Hz]}}$のバターワース型のローパスフィルタを設計し、信号処理しました。カットオフ周波数については事前に周波数のピークなどをFFTで確認し、カットしてもよさそうな周波数を選びました。また位相遅れを考慮して角速度、角加速度だけでなく入力や角度などすべての信号に対してフィルタ処理を行いました。シミュレーションのサンプリング周波数は${1 \mathrm{[kHz]}}$とし、4次のRunge-Kutta法でシミュレーションしました。

表5.2. M系列信号の仕様

項目	値
シフトレジスタ数 ${n}$	8
フィードバックタップ位置	7段目→2段目
シフト周波数 ${\mathrm{[Hz]}}$	13.3
振幅 ${\mathrm{[V]}}$	12
M系列信号の周波数 ${\mathrm{[Hz]}}$	0.143

5.3. 結果

各手法における同定したパラメータを表5.3、実験装置に入力した電圧を図5.2、実験装置と同定したモデルの応答を図5.2に示します。Experimentが実機の結果、LSMが最小二乗法で同定したモデルの結果、PEMが予測誤差法により同定したモデルの結果です。推定したパラメータはどちらの手法も似た値となっています。また応答を見ると、同定した両モデルは角度、角速度共に実機の応答に近いことがわかります。LSMの角速度応答を見ると、立ち上がりなどの過渡的な応答はよく一致していますが、定常値が少し一致していません。対してPEMは立ち上がり、定常値共によく一致しているように見えます。ひとまずシステム同定自体はできてるっぽいです。

表5.3. 同定したモデルのパラメータ

項目	${\alpha}$	${\beta}$	${\gamma}$
LSM	24.6	10.5	41.7
PEM	25.6	16.3	39.4

図5.2. 実験装置に入力した電圧

図5.3. 実験装置と同定したモデルの応答

6. モデルの妥当性評価

先ほど同定したモデルが実際のシステムの応答を表現できるのか、モデルの妥当性について評価してみます。今回、評価は適合率、周波数解析、異なる入力を入れた際の応答の3つで評価します。

6.1. 適合率

適合率とは、システム同定分野で広く使用されている評価指標です。適合率は次式のように表されます。このとき、${y(k)}$と${\hat{y}(k)}$はそれぞれ時刻${k}$での測定値、予測値、${\bar{y}}$は測定値の平均値、${N}$はサンプル数とします。

\mathrm{FIT}=\left\{ 1-\frac{\sqrt{\sum_{k=1}^{N}(y(k)-\hat{y}(k))^2}}{\sqrt{\sum_{k=1}^{N}(y(k)-\bar{y})^2}} \right\} \times 100 \mathrm{[\%]} \tag{6.1}

この適合率の値が大きいほど、同定したモデルは実際のシステムと一致しているということです。5章で同定したモデルの適合率を表6.1に示します。結果を見てみると、どちらも8割越えで一致していることがわかり、よく一致していることがわかります。比較すると、PEMで同定したモデルの方が角度・角速度どちらにおいても、LSMで同定したモデルより適合率が高いです。これだけ見るとPEMの方がうまく同定できているように見えますね。

表6.1. 各手法での適合率 ${\mathrm{[\%]}}$

手法	角度	角速度
LSM	81.5	86.7
PEM	86.2	87.2

6.2. 周波数解析

角度、角速度をFFTした結果を図6.1、6.2に示します。図を見てみると、細かい振幅の違いはありますが、周波数のピークは一致していることが確認できます。このことから、同定したモデルは実際のシステムの周波数特性を表現できているのではないかと考えます。

図6.1. 角度のFFT結果

図6.2. 角速度のFFT結果

6.3. 異なる入力を入れた際の応答

異なる入力として、実験装置に角周波数${2.0 \mathrm{[rad/s]}}$の正弦波入力を印加しました。印加した電圧を図6.3、その応答を図6.4に示します。図のとおり、どちらのモデルも応答がよく一致していることがわかります。PEMで同定したモデルについては角速度はほとんど一致しています。
6.1、6.2、6.3節、これらの結果から、今回同定したモデルは妥当であると考えます。

図6.3. 実験装置に入力した電圧(正弦波入力)

図6.4. 実験装置と同定したモデルの応答(正弦波入力)

まとめ

今回は実際に最小二乗法でシステム同定してみました。システム同定は苦手だったのですが、今回、勉強してみて少し興味が出たんでやってよかったです。使用したコードは最後にのせておきますが、汚いんで解読してやってください。あとこの記事を読んで興味出た！って人はぜひ参考文献を読んでみて下さい。私の記事よりめっちゃ書いてあるんで参考になります。
もし次回を書くなら、今回同定したモデルを使ってコントローラ設計してみようと思います。

参考文献

1. 足立修一，“MATLABによる制御のためのシステム同定”，東京電機大学出版局，(2022)
2. 足立修一，“周波数領域におけるシステム同定の性能評価”，計測と制御，第47巻，第11号，(2008)
3. 室井秀夫，竹下 侑，足立修一，“時間領域におけるシステム同定結果の評価指標について”，計測自動制御学会論文集，Vol51，No.10，p.736-743，(2015)
4. 川田昌克，“MATLAB/Simulinkによる現代制御入門”，森北出版株式会社，(2019)
5. 川田昌克(編著)，東 俊一，市原裕之，浦久保孝光，大塚敏之，甲斐健也，國松禎明，澤田賢治，永原正章，南 裕樹，“倒立振子で学ぶ制御工学”，森北出版株式会社，(2018)
6. 中溝高好，“線形システムの同定”，計測と制御，Vol.28，No.4，p.291-299，(1989)
7. 梶本裕之，“最小二乗法とフィッティングとモデルパラメータ推定について”，Kajimoto Laboratory，(2002)，https://kaji-lab.jp/kajimoto/leastsquare.htm
   (参照 2024年5月31日)
8. @fumiya_sato，“SILS環境の構築のためのDCモータのパラメタ同定及び制御実験入門”，Qiita，(2017)，https://qiita.com/fumiya_sato/items/dc6c37d4e620b514a1a4
   (参照 2024年6月2日)
9. @Carter，“LEGO 部品を利用した回転型倒立振子のレシピを公開！（第 4 回：パラメータ同定 ー MATLAB / Simulink）”，Qiita，(2022)，https://qiita.com/Carter/items/80a1ed5d09f9dc420871
   (参照 2024年6月2日)

サンプルコード

SystemIdentification.py

import pandas as pd
from pathlib import Path
import numpy as np
from matplotlib import pyplot as plt
from scipy.signal import detrend, StateSpace, lsim, lti, TransferFunction, bode
from scipy.optimize import minimize
import Lib_SI as SI

### CSVからデータ取得 ###
file_path = 'Experimental_data/20240525/2024052502.csv'
parent = Path(__file__).resolve().parent
df = pd.read_csv(parent.joinpath(file_path))

### 中心差分近似 ###
dt = 0.005
fs = 1 / dt
N = 2 ** 11
t = df.iloc[0:N, 0] * dt
t = t.to_numpy();                   t = t.T
u = df.iloc[0:N, 1].to_numpy();     u = u.T
theta = df.iloc[0:N, 2].to_numpy()
dtheta = SI.central_diff(theta, dt)
ddtheta = SI.central_diff(dtheta, dt)
y = np.array([theta, dtheta]).T

### ローパスフィルタ ###
fcut = 35
u_f = SI.butter_lowpass_filter(u, fcut, fs, order=4)
theta_f = SI.butter_lowpass_filter(theta, fcut, fs, order=4)
dtheta_f = SI.butter_lowpass_filter(dtheta, fcut, fs, order=4)
ddtheta_f = SI.butter_lowpass_filter(ddtheta, fcut, fs, order=4)

### 最小二乗法 ###
Mf1 = np.array([-dtheta_f, -np.sign(dtheta_f), u_f]).T
Nf1 = np.array([ddtheta_f]).T
p = np.dot(np.linalg.pinv(Mf1), Nf1)
print(p)

# 運動方程式
def func(x, dx, u, a = p[0], b = p[1], c = p[2]):
    return - a * dx - b * np.sign(dx) + c * u

# 初期条件・シミュレーション
x0 = 0
v0 = 0
x, dx = SI.euler_method(t, dt, x0, v0, func, u)
xl, dxl = SI.runge_kutta_method(t, dt, x0, v0, func, u)
##############################################################################
### 予測誤差法 PEM ###
# 目的関数
def objective_function(params, t, dt, x0, v0, u, y_true):
    def motor_dynamics(x, dx, u, a=params[0], b=params[1], c=params[2]):
        return -a * dx - b * np.sign(dx) + c * u
    x_pred, dx_pred = SI.runge_kutta_method(t, dt, x0, v0, motor_dynamics, u)
    error = y_true - x_pred
    return np.mean(error**2)
# 初期パラメータの設定
initial_params = np.array([24.57853985, 10.50384479, 41.69472292])
# 最適化の実行
result = minimize(objective_function, initial_params, args=(t, dt, x0, v0, u, theta_f), method='Nelder-Mead')
print(result)
print(result.x)

# 運動方程式
def func1(x, dx, u, a = result.x[0], b = result.x[1], c = result.x[2]):
    return - a * dx - b * np.sign(dx) + c * u
xlp, dxlp = SI.runge_kutta_method(t, dt, x0, v0, func1, u)
##############################################################################

### 適合率計算 ###
fit_val = np.zeros(6)
fit_val[0] = SI.fit_ratio(theta, x)
fit_val[1] = SI.fit_ratio(dtheta, dx)
fit_val[2] = SI.fit_ratio(theta, xl)
fit_val[3] = SI.fit_ratio(dtheta, dxl)
fit_val[4] = SI.fit_ratio(theta, xlp)
fit_val[5] = SI.fit_ratio(dtheta, dxlp)
print(fit_val)

## FFT ###
ff = plt.figure(figsize=(12,5))
Amp, freq = SI.FFT(theta, fs)
SI.FFT_plot_set(Amp, freq, len(dtheta), 'Experiment', ff)
Amp, freq = SI.FFT(xl, fs)
SI.FFT_plot_set(Amp, freq, len(dxl), 'LSM', ff)
Amp, freq = SI.FFT(xlp, fs)
SI.FFT_plot_set(Amp, freq, len(dxlp), 'PEM', ff)


### グラフプロット ###
fsize = 12
f = plt.figure(figsize=(12,5))
plt.plot(t, u, linewidth=2)
plt.xlabel('Time [s]', fontsize=fsize)
plt.ylabel('Voltage [V]', fontsize=fsize)
plt.tick_params(labelsize=fsize)
plt.grid()

fig, ax = plt.subplots(2, 1, figsize=(12,7.5), squeeze=False)
ax[0, 0].plot(t, theta, label="Experiment", linewidth=2)
ax[0, 0].plot(t, xl, label="LSM", linewidth=2)
ax[0, 0].plot(t, xlp, label="PEM", linewidth=2)
ax[0, 0].set_xlabel('Time [s]', fontsize=fsize)
ax[0, 0].set_ylabel('Angle [rad]', fontsize=fsize)
ax[0, 0].tick_params(labelsize=fsize)
ax[0, 0].grid(True)
ax[1, 0].plot(t, dtheta, label="Experiment", linewidth=2)
ax[1, 0].plot(t, dxl, label="LSM", linewidth=2)
ax[1, 0].plot(t, dxlp, label="PEM", linewidth=2)
ax[1, 0].set_xlabel('Time [s]', fontsize=fsize)
ax[1, 0].set_ylabel('Angular velocity [rad/s]', fontsize=fsize)
ax[1, 0].tick_params(labelsize=fsize)
ax[1, 0].grid(True)
ax[0, 0].legend()
ax[1, 0].legend()
plt.show()

Lib_SI.py

import numpy as np
import matplotlib.pyplot as plt
from scipy import signal

### 高速フーリエ変換 ###
def FFT(data, fs):
    N = len(data)
    window = signal.hann(N)
    F = np.fft.fft(data * window)
    freq = np.fft.fftfreq(N, d=1/fs)    # 周波数スケール
    F = F / (N / 2)                     # フーリエ変換の結果を正規化
    F = F * (N / sum(window))           # 窓関数による振幅減少を補正する
    Amp = np.abs(F)                     # 振幅スペクトル
    return Amp, freq

def FFT_plot_set(Amp, freq, N, labelname, ff=None):
    if ff is None:
        ff = plt.figure()
    else:
        plt.figure(ff.number)
    
    plt.plot(freq[:N//2], Amp[:N//2], label=labelname, linewidth=2)
    plt.xlabel("Frequency [Hz]", fontsize=12)
    plt.ylabel("Amplitude", fontsize=12)
    plt.xlim(0,None)
    plt.ylim(0,None)
    plt.tick_params(labelsize=12)
    plt.grid()
    plt.legend()


### ローパスフィルタ用関数 ###
def butter_lowpass_filter(x, lowcut, fs, order=4):
    nyq = 0.5 *fs
    low = lowcut / nyq
    b, a = signal.butter(order, low, btype='low')
    y = signal.filtfilt(b, a, x)
    return y

### ハイパスフィルタ用関数 ###
def butter_highpass_filter(x, highcut, fs, order=5):
    nyq = 0.5 * fs
    high = highcut / nyq
    b, a = signal.butter(order, highcut, btype='high')
    y = signal.filtfilt(b, a, x)
    return y

### 中心差分近似 ###
def central_diff(x, dt):
    k = len(x)
    h = dt
    dx = np.zeros(k)
    dx[0] = (-3 * x[0] + 4 * x[1] - x[2]) / (2 * h)
    for i in range (1, k-1):
        dx[i] = (x[i+1] - x[i-1]) / (2 * h)
    dx[k-1] = (x[k-3] - 4 * x[k-2] + 3 * x[k-1]) / (2 * h)
    return dx

### オイラー法 ###
def euler_method(t, dt, x0, v0, func, u):
    dx = np.zeros(len(t))
    x = np.zeros(len(t))
    for i in range(len(t) - 1):
        if i == 0:
            dx[i] = v0
            x[i] = x0
        dx[i + 1] = dx[i] + func(x[i], dx[i], u[i]) * dt
        x[i + 1] = x[i] + dx[i] * dt
    return x, dx

### ルンゲクッタ法 ###
def runge_kutta_method(t, dt, x0, v0, func, u):
    dx = np.zeros(len(t))
    x = np.zeros(len(t))
    for i in range(len(t) - 1):
        if i == 0:
            dx[i] = v0
            x[i] = x0
        k1_x = dx[i] * dt
        k1_v = func(x[i], dx[i], u[i]) * dt
        k2_x = (dx[i] + k1_v / 2) * dt
        k2_v = func(x[i] + k1_x / 2, dx[i] + k1_v / 2, u[i]) * dt
        k3_x = (dx[i] + k2_v / 2) * dt
        k3_v = func(x[i] + k2_x / 2, dx[i] + k2_v / 2, u[i]) * dt
        k4_x = (dx[i] + k3_v) * dt
        k4_v = func(x[i] + k3_x, dx[i] + k3_v, u[i]) * dt

        dx[i + 1] = dx[i] + (k1_v + 2 * k2_v + 2 * k3_v + k4_v) / 6
        x[i + 1] = x[i] + (k1_x + 2 * k2_x + 2 * k3_x + k4_x) / 6
    return x, dx

# 運動方程式 上記のfuncはこんな感じで運動方程式を関数として定義して使って
# def func(x, dx, u, a = p[0], b = p[1], c = p[2]):
#     return - a * dx - b * np.sign(dx) + c * u

### 適合率(システム同定の評価) ###
def fit_ratio(y_actual, y_model):
    """
    適合率を計算する関数
    Parameters:
    y_actual (np.array): 実測データの出力ベクトル
    y_model (np.array): 同定したモデルの出力ベクトル
    Returns:
    float: 適合率（百分率）
    """
    # 実測データの平均値
    y_mean = np.mean(y_actual)

    # 分母：実測データとその平均値との差の二乗和の平方根
    denominator = np.linalg.norm(y_actual - y_mean)
    # 分子：実測データとモデル出力の差の二乗和の平方根
    numerator = np.linalg.norm(y_actual - y_model)

    # 適合率の計算
    fit = (1 - (numerator / denominator)) * 100
    return fit