IMUセンサ,オイラー角でカルマンフィルタ設計

センサ

前回の記事では角度を求めるために、次に示すモデル式をオイラー法により求めた。ブロック線図を図１に示す。

\begin{gather}
\begin{bmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{bmatrix}=
\begin{bmatrix}
1&\sin{\phi}\tan{\theta}&\cos{\phi}\tan{\theta}\\
0&\cos{\phi} &-\sin{\phi}\\
0&\frac{\sin{\phi}}{\cos{\theta}}& \frac{\cos{\phi}}{\cos{\theta}}
\end{bmatrix}
\begin{bmatrix}
w_{x}\\
w_{y}\\
w_{z}
\end{bmatrix}\tag{1}
\end{gather}

図1 オイラー法による角度の推定を示したブロック線図

この記事では拡張カルマンフィルタを用いて角度を推定する。
用いるモデルは依然と同じ、式(1)である。
今回の推定手法の流れを示したブロック線図を図２に示す。

図2 カルマンフィルタを用いた手法のブロック線図

推定手法

今回使うモデル式は次のようなもの。

\begin{gather}
\begin{bmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{bmatrix}=
\begin{bmatrix}
1&\sin{\phi}\tan{\theta}&\cos{\phi}\tan{\theta}\\
0&\cos{\phi} &-\sin{\phi}\\
0&\frac{\sin{\phi}}{\cos{\theta}}& \frac{\cos{\phi}}{\cos{\theta}}
\end{bmatrix}
\begin{bmatrix}
w_{x}\\
w_{y}\\
w_{z}
\end{bmatrix}\tag{2}
\end{gather}

式(2)は非線形(双線形)であるため今回は状態量である角度を推定するために拡張カルマンフィルタを用いる。ここでは参考文献[1]である足立先生の「カルマンフィルタの基礎」をもとに進めた。
状態量や状態方程式などを図3に示す。またシステム行列の偏微分を示したものを図４に示す。

図3 モデルの概略＿状態方程式
図4 システム行列の偏微分行列

プログラム

拡張カルマンフィルタを用いた角度推定プログラムを以下に示す。


"""
2024/10/18
オイラー角で角度を推定した。
KFで推定する

"""

import csv
import numpy as np  
import os
from datetime import datetime
import time
import serial, struct
from numpy import sin, cos, pi, tan, arctan, sqrt, arcsin,arctan2,arcsin
import matplotlib.pyplot as plt
import pandas as pd
# ----------
path = os.getcwd() + "/csv"
extens = ".csv"
# -----------------------------


ary_IMU = []
firstloop_initial = True

ser = serial.Serial('COM3', 9600, timeout=None) #new ICM

      # ary_IMU= [proc_T,dt,Ax, Ay,Az,Gx, Gy,Gz,Mx, My,Mz,angle_x,angle_y,angle_z,"\n"]
ary_IMU_name = ['proc_T','dt','AX','AY','AZ','Gx','Gy','Gz','Mx','My','Mz',
                'angle_x','angle_y','angle_z','pitch_a','yaw_a','pitch_EKF','yaw_EKF','Just_pitch','Just_yaw','Just_roll','\n']
filename = datetime.now().strftime("%m_%d__%H_%M_%S")
with open(path + "/" + filename + "_" + extens,'a',newline='') as f:
  writer = csv.writer(f)
  writer.writerow(ary_IMU_name)


val = struct.pack("B", 0x01)  # Serial-ON
ser.write(val)

start_T = time.time()
pre_time = start_T
time.sleep(0.01)
try:
  while True:
    cur_time = time.time()
    dt_py = 0.005 #[s]
    dt_Python=0.005
    proc_T = cur_time - start_T

    dt_loop = cur_time - pre_time #[s]
    pre_time = cur_time

    line = ser.readline()
    
    
    stripped_str = str(line, 'utf-8').strip()# byte型を文字列に変換して前後の空白改行除去
    datas = [float(d) for d in stripped_str.split(',')]  # 整数ではなく浮動小数点数に変換する

    if len(datas) == 10:

      Ax = datas[0]
      Ay = datas[1]
      Az = datas[2]
      
      Gx = datas[3]
      Gy = datas[4]
      Gz = datas[5]

      Mx = datas[6]
      My = datas[7]
      Mz = datas[8]

      dt = datas[9]*0.001
      # オフセットの修正
      Ax=(Ax*0.01)-0.719+0.4
      Ay=(Ay*0.01)-0.6  
      Az=(Az*0.01)-0.1

      Gx = Gx-0.348
      Gy = Gy-0.138
      Gz = Gz+0.405

      Mx = Mx*0.001
      My = My*0.001
      Mz = Mz*0.001


      Wx,Wy,Wz = Gx*pi/180, Gy*pi/180, Gz*pi/180  #[rad] G→Wに変更
      Omg_s = np.array([ [Wx],[Wy],[Wz] ])        #[rad/s]

      Acc_s = np.array( [[Ax],[Ay],[Az]] )
      pitch_a = arctan2(Acc_s[1,0],Acc_s[2,0])#x軸回り
      yaw_a = arctan2( -Acc_s[0,0] ,sqrt(Acc_s[1,0]**2 + Acc_s[2,0]**2) )#y軸まわり]

      # 観測値
      angle_acc = np.array([[yaw_a],[pitch_a]])
      one3 = np.ones(3)             #      

      R_e = np.eye(2)*0.01 #観測ノイズ行列
      Q_e = np.eye(3)*0.001  #システムノイズ行列
      P_e = np.eye(2)     #誤差共分散行列 状態量の数の単位行列

      matrix = np.array([[1,sin(pitch_a)*tan(yaw_a),cos(pitch_a)*tan(yaw_a)],
                         [0,cos(pitch_a),-sin(pitch_a)],
                         [0,(sin(pitch_a))/(cos(yaw_a)),(cos(pitch_a))/(cos(yaw_a))]])
    

      if 1 :

                #初期値 Accel-->Theta
                if firstloop_initial:
                    firstloop_initial = False
                    flag_second = True
                    angle_x=pitch_a
                    angle_y=yaw_a
                    Angle_e = np.zeros(3)
                    Angle_kf = np.zeros(2)
                    JustIntegratedAngle=np.zeros(3)
                    

                    # Angle=np.zeros(3)
              
                if flag_second:
                   matrix = np.array([[1,sin(angle_x)*tan(angle_y),cos(angle_x)*tan(angle_y)],
                         [0,cos(angle_x),-sin(angle_x)],
                         [0,(sin(angle_x))/(cos(angle_y)),(cos(angle_x))/(cos(angle_y))]]) 

                phi = angle_x
                theta = angle_y

                sp = sin(phi)
                st = sin(theta)

                cp = cos(phi)
                ct = cos(theta)

                tp = tan(phi)
                tt = tan(theta)

                # 状態方程式の偏微分行列
                F = np.array([[1+dt*(cp*tt*Wy-sp*tt*Wz),dt*(Wy*(sp/(ct**2))+Wz*(cp/(ct**2)))],
                              [dt*(-sp*Wy-cp*Wz),1*dt]])
                

                # 入力
                Uq = np.array([[Wx],[Wy],[Wz]])
                # 離散化したとき一つ前にかける単位行列
                AA = np.array([[1,0],[0,1]])

                # 離散化したときB行列
                BB = np.array([[1,sp*tt,cp*tt],[0,cp,-sp]])
                #出力方程式の偏微分行列
                H = np.eye(2) 

                #EKFの状態量
                Xa = Angle_kf          # Gainあり

                #EKFの状態方程式<予測ステップ>
                Xa_ = AA @ Xa + BB @ Uq # Gainあり

                #EKFの事前誤差共分散行列の更新
                P_e_ = F @ P_e @ F.T + BB @ Q_e @ BB.T  
                #EKFのカルマンゲイン更新
                Kq_num = P_e_ @ H.T                        
                Kq_den = np.linalg.inv( H @ Kq_num + R_e ) 
                Ke = Kq_num @ Kq_den 

                #--C=I only Angle--
                Ye_ = Xa_#modelのoutput
                Ye = angle_acc #accからの観測値

                #EKF<フィルタリングステップ>
                Xe = Xa_ + Ke @ (Ye - Ye_ )# Gainあり
                #事後誤差共分散行列の更新
                P_e = ( np.eye(2) - Ke @ H ) @ P_e_ 

                pitch_EKF = Xe[1,0]
                yaw_EKF = Xe[0,0]
                
                # モデルを後進差分
                Angle_e = Angle_e+dt_Python*(matrix@Omg_s)
                angle_x=Angle_e[0,0]
                angle_y=Angle_e[1,0]
                angle_z=Angle_e[2,0]

                # ジャイロの値をオイラー法で積分したもの
                JustIntegratedAngle=JustIntegratedAngle+dt_Python*(Omg_s*180/pi) 
                Just_pitch=JustIntegratedAngle[0,0]
                Just_yaw=JustIntegratedAngle[1,0]
                Just_roll=JustIntegratedAngle[2,0]

                print("angle_x=",angle_x*180/pi,"angle_y=",angle_y*180/pi,"angle_z=",angle_z*180/pi)

      ary_IMU= [proc_T,dt,Ax, Ay,Az,Gx, Gy,Gz,Mx, My,Mz,angle_x*180/pi,angle_y*180/pi,angle_z*180/pi,pitch_a*180/pi,yaw_a*180/pi,pitch_EKF*180/pi,yaw_EKF*180/pi,Just_pitch,Just_yaw,Just_roll,"\n"]
      with open(path + "/" + filename + "_" + extens,'a',newline='') as f:
         writer = csv.writer(f)
         writer.writerow(ary_IMU)  # ary_IMUを直接渡す
    
    else:
      print('not 10 data')

except KeyboardInterrupt:
  

  val = struct.pack("B", 0x02)  # Serial-OFF
  ser.write(val)
  time.sleep(0.5)
  print("saved")

# Read CSV file and create a scatter plot
df = pd.read_csv(path + "/" + filename + "_" + extens, delimiter=',')

time_csv=df['proc_T']

# Acc
AX_csv=df['AX']
AY_csv=df['AY']
AZ_csv=df['AZ']

# Gyro
Gx_csv=df['Gx']
Gy_csv=df['Gy']
Gz_csv=df['Gz']

# Mag
Mx_csv=df['Mx']
My_csv=df['My']
Mz_csv=df['Mz']

# angle Eular method モデルを後進差分
angle_x_csv=df['angle_x']
angle_y_csv=df['angle_y']
angle_z_csv=df['angle_z']

# angle just integrated
angle_inte_x_csv=df['Just_pitch']
angle_inte_y_csv=df['Just_yaw']
angle_inte_z_csv=df['Just_roll']

# accからのpitch/yaw
pitch_a_csv=df['pitch_a']
yaw_a_csv=df['yaw_a']

pitch_EKF_csv=df['pitch_EKF']
yaw_EKF_csv=df['yaw_EKF']
size = 2
FONTSIZE=12


"""
＝＝＝＝＝センサの生データ
"""
fig, axes = plt.subplots(3, 1, tight_layout=True)
axes[0].scatter(time_csv,Mx_csv,label='x',s=size)
axes[0].scatter(time_csv,My_csv,label='y',s=size)
axes[0].scatter(time_csv,Mz_csv,label='z',s=size)
axes[0].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[0].set_ylabel('Mag/T',fontsize=FONTSIZE)
axes[0].legend()
axes[0].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[0].grid()

axes[1].scatter(time_csv,AX_csv,label='x',s=size)
axes[1].scatter(time_csv,AY_csv,label='y',s=size)
axes[1].scatter(time_csv,AZ_csv,label='z',s=size)
axes[1].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[1].set_ylabel('Acc/m/s/s',fontsize=FONTSIZE)
axes[1].legend()
axes[1].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[1].grid()

axes[2].scatter(time_csv,Gx_csv,label='x',s=size)
axes[2].scatter(time_csv,Gy_csv,label='y',s=size)
axes[2].scatter(time_csv,Gz_csv,label='z',s=size)
axes[2].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[2].set_ylabel('Gyro/deg/s',fontsize=FONTSIZE)
axes[2].legend()
axes[2].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[2].grid()

"""
angle_just_inte/deg  angle_EKF/deg
angle_acc/deg  angle_eular/deg
"""
fig, axes = plt.subplots(2, 2, tight_layout=True)
fig.suptitle(filename)
axes = axes.flatten()

# angle_just_inte
axes[0].scatter(time_csv,angle_inte_x_csv,label='x',s=size)
axes[0].scatter(time_csv,angle_inte_y_csv,label='y',s=size)
axes[0].scatter(time_csv,angle_inte_z_csv,label='z',s=size)
axes[0].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[0].set_ylabel('angle_inte/deg',fontsize=FONTSIZE)
axes[0].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[0].grid()


# angle==EKF==
axes[1].scatter(time_csv,pitch_EKF_csv,label='x',s=size)
axes[1].scatter(time_csv,yaw_EKF_csv,label='y',s=size)
axes[1].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[1].set_ylabel('angle_EKF/deg',fontsize=FONTSIZE)
axes[1].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[1].grid()

# angle<加速度から>
axes[2].scatter(time_csv,pitch_a_csv,label='x',s=size)
axes[2].scatter(time_csv,yaw_a_csv,label='y',s=size)
axes[2].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[2].set_ylabel('angleAcc/deg',fontsize=FONTSIZE)
axes[2].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[2].grid()

# angle<オイラー法>モデルを後進差分
axes[3].scatter(time_csv,angle_x_csv,label='x',s=size)
axes[3].scatter(time_csv,angle_y_csv,label='y',s=size)
axes[3].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[3].set_ylabel('angleEular/deg',fontsize=FONTSIZE)
axes[3].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[3].grid()


"""
＝＝＝＝＝観測値とEKFデータを１つの図で比較する
"""
fig, axes = plt.subplots(2, 1, tight_layout=True)
axes[0].scatter(time_csv,pitch_EKF_csv,label='pitch-EKF',s=size)
axes[0].scatter(time_csv,pitch_a_csv,label='pitch-acc',s=size)
axes[0].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[0].set_ylabel('pitch-compare/deg',fontsize=FONTSIZE)
axes[0].legend()
axes[0].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[0].grid()

axes[1].scatter(time_csv,yaw_EKF_csv,label='yaw-EKF',s=size)
axes[1].scatter(time_csv,yaw_a_csv,label='yaw-acc',s=size)
axes[1].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[1].set_ylabel('yaw-compare/deg',fontsize=FONTSIZE)
axes[1].legend()
axes[1].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[1].grid()

plt.show()

実験とその結果

上のプログラムを使ってIMUセンサをX軸(pitch)、Y軸(yaw)回りに回転させた。
その時の結果を図5に示す。
左上は角速度の値を単純に積分することで求めた角度
右上は拡張カルマンフィルタで推定した角度
左下は加速度をもとに推定した角度
右下はモデルを単純に後進差分(オイラー法)したことで求めた角度である。

図5 実験結果

観測値とEKFの結果は似ている。以下の図６にその比較結果を示す。
上がX軸周り(pitch)、下がY軸回り(yaw)の比較である。
大体同じだが、観測ノイズを今より大きい値にするとEKFの結果がよりノイズっぽくなる。(ジャイロの値を入力とするモデルの予測値に重みが大きくなるといった方が正確かもしれないが。)
対して観測ノイズを小さくすると、観測値とEKFの出力がほとんど同じになる。
今回はジャイロと加速度の取得できるサンプリング周波数は同じであるが、観測値のサンプリング周波数が下がったり、モデルの入力のサンプリング周波数が上がったりして差が出ると、より違いが確認されるであろう。(多分)

図6 観測結果と拡張カルマンフィルタの推定結果の比較

参考文献

[1] カルマンフィルタの基礎　足立修一, 丸田一郎　電機大出版局, 2017

コメント

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