ローパスフィルタ、ハイパスフィルタのプログラム

センサ

センサからデータをとるとどうしても望まないノイズや定常値が出てしまうことがあります。
そんな時にフィルタは使われますが、プログラムで書くとどのように表せるのでしょうか。

想定読者

・ローパスフィルタ、ハイパスフィルタを作りたいが方法を知りたい方
・制御工学の初心者の方
・古典制御を勉強している方
・センサを使う場面が多い方

ローパスフィルタ

高周波な雑音ノイズを取り除きたいときによくローパスフィルタが使われますね。

ローパスフィルタの伝達関数は以下のように示せます。
\begin{align}
H(s)=\frac{K}{1+Ts}
\end{align}

$T$は時定数、$K$はゲインで、ゲインは定数倍ですので必要に応じて変更します。
これを状態空間表現に直していきます。

\begin{align}
X(s)&=\frac{1}{1+Ts}U(s),\tag{1}\\
Y(s)&=K*X(s)\tag{2}
\end{align}

上のように分割し、それぞれ逆ラプラス変換します。
まず(1)式は、
\begin{align}
&X(s)\{1+Ts\}=U(s)\\
&x(t)+T\dot{x}(t)=u(t)\\
&\dot{x}(t)=-\frac{1}{T}X(t)+\frac{1}{T}u(t)
\end{align}

(2)式は、
\begin{align}
y(t)=Kx(t)
\end{align}

これをプログラムを使って確認していきます。
下は2000個のノイズをローパスフィルタに通した結果になります。
ローパスフィルタは高周波成分を除去する働きがありますが、時定数を大きくすると、その分だけ遅れが生じてしまいますので使う場面ではその点を考慮する必要があります。

import numpy as np
import matplotlib.pyplot as plt

# 問題設定
A = 1
b = 1
c = 1
Q = 1
R = 2
N = 2001
t_noise = np.arange(1, N + 1)

Tsim = 20
sampleT = 0.01
Tn = round(Tsim / sampleT)

# 観測データの生成
# 雑音信号の生成
v = np.random.randn(N, 1) * np.sqrt(Q)
w = np.random.randn(N, 1) * np.sqrt(R)
# 状態空間モデルを用いた時系列データの生成
x = np.zeros((N, 1))
y = np.zeros((N, 1))
y[0] = (c*x[0, :]) + w[0]
for k in range(1, N):
    # x[k+1] = (A@ x[k]) + b * v[k]
    x[k, :] = (A* x[k-1, :]) + b * v[k-1]
    y[k] = (c* x[k, :]) + w[k]

T = 0.05
A = -1 / T
B = 1 / T
K = 1
x0N = 0
x_sn = np.zeros((N + 1, 1))
x_sn[0] = x0N
t = np.zeros(Tn + 1)
t_o_N = np.zeros(Tn + 1)
x_o_N = np.zeros(Tn + 1)

for i in range(Tn):
    k1 = A * x_sn[i] + B * y[i]
    k2 = A * (x_sn[i] + sampleT * k1 / 2) + B * y[i]
    k3 = A * (x_sn[i] + sampleT * k2 / 2) + B * y[i]
    k4 = A * (x_sn[i] + sampleT * k3) + B * y[i]
    x_sn[i + 1] = x_sn[i] + sampleT * (k1 + 2 * k2 + 2 * k3 + k4) / 6
    x_o_N[i] = K * x_sn[i]

# プロット
plt.figure()
plt.plot(t_noise, y, linewidth=1, label='noise')
plt.plot(t_noise, x_o_N, linewidth=1.5, label='LPF')
plt.legend(fontsize=15)
plt.xlabel('time', fontsize=15)
plt.ylabel('noize', fontsize=15)
plt.xlim([0, 2000])
plt.show()

コメント

リアルタイムでないのであればローパスフィルタをFIRフィルタの移動平均フィルタ
にしたほうが遅れに悩まされることはありません。

ハイパスフィルタ

ハイパスフィルタは生データからローパスフィルタを引いた値になります。
また、ローパスフィルタが高周波をカットする一方でハイパスフィルタは低周波をカットします。
どんな時に使うのかというと、定常値が常に出てしまい、定常値からの差を知りたい時などです。

例えば、Z軸に加速度が9.8[m/s/s]かかっている場合を想定します。
x軸やy軸は通常0で振ると加速度が加わります。
下の図はZ→X→Yの順番で加速度を与えたときの生データになります。

ここから例えば速度を求めたいときにこのまま積分してしまうと以下のようになります。
上の加速度の面積が積分である速度に値するため、Z軸の速度はどんどん発散してしまいます。

そこで登場するのがハイパスフィルタです。
加速度にハイパスフィルタを通す処理が以下になります。

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

df = pd.read_csv('02_12__14_51_51_.csv')

AX=df['AX']
AY=df['AY']
AZ=df['AZ']
time=df['proc_T']
dt=0.005

AX=AX.to_numpy()
AY=AY.to_numpy()
AZ=AZ.to_numpy()
A=np.array([[AX],[AY],[AZ]])
time=time.to_numpy()

VX_LPF=np.zeros_like(AX)
VY_LPF=np.zeros_like(AY)
VZ_LPF=np.zeros_like(AZ)
V_LPF=np.array([[VX_LPF],[VY_LPF],[VZ_LPF]])


#ハイパスフィルタ=生データ-ローパスフィルタ
T_LPF=0.1
A_Mat = np.eye(3) * (-1/T_LPF)
B_Mat = np.eye(3) * (1/T_LPF)

Xp=V_LPF

for i in range(A.shape[2]-1):
    k1 = A_Mat @ Xp[:,:, i] + B_Mat @ A[:, :, i]
    k2 = A_Mat @ (Xp[:,:, i] + 0.5*dt*k1) + B_Mat @ A[:, :, i]
    k3 = A_Mat @ (Xp[:,:, i] + 0.5*dt*k2) + B_Mat @ A[:, :, i]
    k4 = A_Mat @ (Xp[:,:, i] + 1.0*dt*k3) + B_Mat @ A[:, :, i]
    Xp[:, :,i+1] = Xp[:, :,i] + (k1 + 2*k2 + 2*k3 + k4) * dt / 6
A_LPF=Xp


#初期立ち上がり時間が入るため引いています。
n=50
A[:,:,n:]
A_HPF=A[:,:,n:]-A_LPF[:,:,n:]


plt.plot(time[n:],A_HPF[0][0],label='x')
plt.plot(time[n:],A_HPF[1][0],label='y')
plt.plot(time[n:],A_HPF[2][0],label='z')

plt.title('acc of HPF')
plt.xlabel('time[s]')
plt.ylabel('acc[m/ss]')
plt.legend()
# Highpasssfilterを通したあとの加速度
plt.grid()

このグラフを見ると定常値がなくなって偏差だけが残っていることがわかります。

なぜか

まずローパスフィルタは連続時間の移動平均ととらえるとわかりやすいです。
ハイパスフィルタ=生データー平均値とざっくり考えると重力加速度の定常値が
なくなった理由がイメージできます。

波という観点では、定常値を”長周期”考えるとハイパスフィルタの名前の通りであることが分かります。


ちなみに, dfはこのようなデータフレームです。

さて、上で求めたハイパスフィルタを通した加速度を積分して速度を求めると、次のようになります。

for i in range(A.shape[2]-1-n):
    V_HPF[:,:,i+1]=V_HPF[:,:,i]+(dt*A_HPF[:,:,i])

plt.plot(time,V_HPF[0][0],label='x')
plt.plot(time,V_HPF[1][0],label='y')
plt.plot(time,V_HPF[2][0],label='z')


plt.title('velocity by Integrated Acc of HPF')
plt.xlabel('time[s]')
plt.ylabel('velocity[m/s]')
plt.legend()
plt.grid()

最初に示した速度よりはだいぶましになったことがわかります。
今後はさらに精度を高めたフィルタを紹介していきたいと考えています。

雑談

今回はルンゲクッタ法の入力と出力は同じ物理量でしたが、A行列やB行列といったものに
物理的意味を持つ定数などが加わると、出力と入力で単位が変わります。
(時間変化を表現し、ざっくり言えば、積分することができます。)

伝達関数から

前の章ではローパスフィルタから引く方法でハイパスフィルタを設計しました。
ここでは直接求めていきます。
ハイパスフィルタの伝達関数は以下のようにあらわせます。

\begin{align}
H(s)=\frac{sT}{1+sT}
\end{align}
最初に分母と分子に$\frac{1}{T}$をかけると
\begin{align}
H(s)=\frac{s}{\frac{1}{T}+s}
\end{align}
となります。
$\frac{1}{T}=w$と置くと、
\begin{align}
H(s)=\frac{s}{s+w}
\end{align}と定義できます。

ここから、ローパスフィルタを求めたときと同様に状態空間表現に直していきます。

\begin{align}
&X(s)=\frac{s}{s+w}U(s)\tag{3}\\
&Y(s)=sX(s)\tag{4}\\
\end{align}

(3)式は次のように直すことができる。
\begin{align}
&X(s)=\frac{s}{s+w}U(s)\\
&X(s)\{s+w\}=U(s)\\
\end{align}
逆ラプラス変換すると
\begin{align}
&\dot{x(t)}+wx(t)=u(t)\\
&\dot{x}(t)=-wx(t)+u(t)
\end{align}

(4)式は
\begin{align}
y(t)=\dot{x}(t)
\end{align}

以上の式をプログラムで確認していきます。

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import japanize_matplotlib  
df = pd.read_csv('02_12__14_51_51_.csv')

AX=df['AX'].to_numpy()
AY=df['AY'].to_numpy()
AZ=df['AZ'].to_numpy()
time=df['proc_T']
dt=0.005

A=np.array([[AX],[AY],[AZ]])
time=time.to_numpy()

AX_HPF=np.zeros_like(AX)
AY_HPF=np.zeros_like(AY)
AZ_HPF=np.zeros_like(AZ)
A_HPF=np.array([[AX_HPF],[AY_HPF],[AZ_HPF]])

wn=10
A_HPF_Mat = np.eye(3)*(-wn)
B_HPF_Mat = np.eye(3)
Xp=A_HPF



for i in range(A.shape[2]-1):
    k1 = A_HPF_Mat @ Xp[:,:, i] + B_HPF_Mat @ A[:, :, i]
    k2 = A_HPF_Mat @ (Xp[:,:, i] + 0.5*dt*k1) + B_HPF_Mat @ A[:, :, i]
    k3 = A_HPF_Mat @ (Xp[:,:, i] + 0.5*dt*k2) + B_HPF_Mat @ A[:, :, i]
    k4 = A_HPF_Mat @ (Xp[:,:, i] + 1.0*dt*k3) + B_HPF_Mat @ A[:, :, i]
    Xp[:, :,i+1] = Xp[:, :,i] + (k1 + 2*k2 + 2*k3 + k4) * dt / 6
    A_HPF[:,:,i]=-wn*Xp[:,:,i]+A[:,:,i]
    
fig,axes=plt.subplots(1,2,figsize=(15,10))
ax=axes.flatten()

ax[0].set_title('加速度の生データ')
ax[0].plot(time,A[0][0],label='x')
ax[0].plot(time,A[1][0],label='y')
ax[0].plot(time,A[2][0],label='z')
ax[0].set_xlabel('time[s]')
ax[0].set_ylabel('row-acc[m/s/s]')
ax[0].legend()
ax[0].grid()

ax[1].set_title('HPFを通した加速度')
ax[1].plot(time,A_HPF[0][0],label='x')
ax[1].plot(time,A_HPF[1][0],label='y')
ax[1].plot(time,A_HPF[2][0],label='z')
ax[1].set_xlabel('time[s]')
ax[1].set_ylabel('acc HPF[m/s/s]')
ax[1].legend()
ax[1].grid()

以上のプログラムを前に示した加速度データに作用させると以下のように定常値がカットされます。

コメント

タイトルとURLをコピーしました