カルマンフィルタを用いた姿勢推定

はじめに

6軸センサ(3軸加速度,3軸ジャイロ)でカルマンフィルタを使った姿勢推定器を実装してみました。メモ程度。

#使用するハードウェア
RaspberryPi3+Navio2

姿勢について

今回、姿勢表現にはオイラー角を使います。てことで回転行列はこんな感じです。
$z$軸周りに$\phi$、$y$軸周りに$\theta$、$x$軸周りに$\psi$の順で回転させています。
$$
R(\Phi)=\begin{bmatrix}
C_\phi C_\theta& C_\phi S_\theta S_\psi-S_\phi C_\psi &C_\phi S_\theta C_\psi+S_\phi S_\psi \
S_\phi C_\theta& S_\phi S_\theta S_\psi+C_\phi C_\psi &S_\phi S_\theta C_\psi-C_\phi S_\psi \
-S_\theta& C_\theta\ S_\phi &C_\theta C_\psi
\end{bmatrix}
$$
ここで、$\Phi=[\phi,\theta,\psi]^T$で$S_\times$ $C_\times$はsinとcosてことでおねがいします。これによって姿勢推定されるボディの座標系$\Sigma_b$と慣性座標系$\Sigma_o$の関係はこんな感じです。
$$R(\Phi)[\Sigma_b]=[\Sigma_o]$$
また、ボディの角速度$\omega^b$とオイラー角の時間微分$\dot{\Phi}$の関係はこんな感じです。
$$\omega^b=Q\dot{\Phi}\
Q=\begin{bmatrix}
-S_\theta&0&1\
C_\theta S_\psi& C_\psi&0\
C_\theta C_\psi& -S_\psi&0
\end{bmatrix}$$
この作業を雑にすると座標がこんがらがるんで自分なりに回転順番とかどっちの座標からどっちの座標への変換とか考えることが大事ですね。まじで。
参考文献:https://repository.exst.jaxa.jp/dspace/bitstream/a-is/23926/1/naltm00636.pdf

#拡張カルマンフィルタ
拡張カルマンフィルタ設計していきます。まず、状態空間モデルはこんな感じ。
$$
x_{n+1}=f(x_n, u_n)+v\
y_{n}=x_n+w\
f(x_n,u_n)=\begin{bmatrix}
\theta_{n}+(C_{\psi_n}\omega_y-S_{\psi_n}\omega_z)T_s\
\psi_n+(\omega_x+S_{\psi_n}T_{\theta_n}+C_{\psi_n}T_{\theta_n}\omega_z)T_s
\end{bmatrix}\
y_n=\begin{bmatrix}
\arctan (\frac{-a_{xn}}{\sqrt{a_{yn}^2+a_{zn}^2}})\
\arctan (\frac{a_{yn}}{a_{zn}})
\end{bmatrix}
$$
ここで$x_n=[\theta_n,\psi_n]$で,T_Sはステップ時間,$a_n=[a_{xn},a_{yn},a_{zn}]$は加速度センサの値、$\omega~[\omega_x,\omega_y,\omega_z]$はジャイロセンサから得た角速度,$v,w$は白色雑音です。
Z軸周りの回転角である$\phi$は加速度センサから角度を算出できないのでカルマンフィルタで推定できません。積分して求めるしかないですね。
とりあえず、1式目はジャイロセンサの値を$Q$の逆行列かけて変化して積分してる式で、2式目は加速度センサから求めた姿勢角に関する式ですね。
モデルができればあとは離散拡張カルマンフィルタに当てはめていくだけです。

参考文献:カルマンフィルタの基礎

#実装
参考程度にコードを載せます。(あくまで参考程度)

functions.py

import numpy as np

def get_angle_acc(acc):
    th_acc=np.arctan2(-acc[0],np.sqrt(acc[1]*acc[1]+acc[2]*acc[2]))
    ps_acc=np.arctan2(acc[1],acc[2])
    y=np.array([th_acc,ps_acc])
    return y

def get_Kalamgain(P,c,r):
    CPC=np.dot(c,np.dot(P,c))+r
    return np.dot(P,np.dot(c,np.linalg.inv(CPC)))

def get_preEstimation2(x,gyro,Ts,Tri):
    Q=np.array([[0,Tri[1,0],-Tri[1,1]],[1,Tri[1,1]*Tri[0,2],Tri[1,0]*Tri[0,2]]])
    return x+np.dot(Q,gyro)*Ts

def get_preVariance2(x,gyro,P,b,q,Ts,Tri):
    A=np.array([[1,-(Tri[1,1]*gyro[1]+Tri[1,0]*gyro[2])*Ts],[(Tri[1,0]/Tri[0,0]/Tri[0,0]*gyro[1]-Tri[1,1]/Tri[0,0]/Tri[0,0]*gyro[2])*Ts,1+(Tri[1,0]*Tri[0,2]*gyro[1]-Tri[1,1]*Tri[0,2]*gyro[2])*Ts]])
    return np.dot(A,np.dot(P,A.transpose()))+q

def Kalman_filer2(x,y,gyro,c,b,q,r,P,Ts,Tri):
    x_=get_preEstimation2(x,gyro,Ts,Tri)
    P_=get_preVariance2(x,gyro,P,b,q,Ts,Tri)
    g=get_Kalamgain(P_,c,r)
    return x_+np.dot(g,y-np.dot(c,x_)),get_Variance(g,c,P_)

def Jacobian_forprocessvariance2(Tri):
    return np.array([[0,Tri[1,0],-Tri[1,1]],[1,Tri[1,1]*Tri[0,2],Tri[1,0]*Tri[0,2]]])

def get_Trigonometrxic(x):
    return np.array([[np.cos(x[0]),np.sin(x[0]),np.tan(x[0])],[np.cos(x[1]),np.sin(x[1]),np.tan(x[1])]])

main.py

import time
import argparse 
import navio.mpu9250
import numpy as np
import functions as fcn

IMU = navio.mpu9250.MPU9250()

IMU.initialize(1,0x06)#lowpass->20Hz
time.sleep(1)

m6a0=np.zeros((3,))
m6g0=np.zeros((3,))

for i in range(1,1000):
    ma0, mg0 = IMU.getMotion6()
    m6a0=m6a0+np.array(ma0)
    m6g0=m6g0+np.array(mg0)
m6g0=m6g0/1000
m6a0=m6a0/1000
m6a0[0],m6a0[1]=m6a0[1],m6a0[0]
m6g0[0],m6g0[1]=m6g0[1],m6g0[0]

x0=np.zeros((2,))
for i in range(1,1000):
    m6a, m6g = IMU.getMotion6()
    m6a[0],m6a[1]=-m6a[1],-m6a[0]      
    m6a=np.array(m6a)
    x0=x0+fcn.get_angle_acc(m6a)
x0=x0/1000

x=x0
P=np.zeros((2,2))
Ts=1.0/250.0
Yaw=0
Tri=np.zeros((2,3))

c=np.array([[1,0],[0,1]])
q=np.array([[1.74E-3*Ts*Ts,0],[0,1.74E-3*Ts*Ts]])
b=np.array([[1,0],[0,1]])
r=np.array([[1*Ts*Ts,0],[0,1*Ts*Ts]])
Cgy=np.eye(3,3)*1.74E-3*Ts*Ts

while(True):
 t_estimate_Attitude0=time.time()
 m6a, m6g = IMU.getMotion6()
 m6a[0],m6a[1]=-m6a[1],-m6a[0]
 m6g[0],m6g[1]=m6g[1],m6g[0]
                    
 m6a=np.array(m6a)
 m6g=np.array(m6g)-m6g0
 m6g[2]=-m6g[2]

 Tri=fcn.get_Trigonometrxic(x)
 J=fcn.Jacobian_forprocessvariance2(Tri)
 Jt=J.transpose()
 x,P=fcn.Kalman_filer2(x,fcn.get_angle_acc(m6a),m6g,c,b,q,r,P,Ts,Tri)
 Yaw=Yaw+(Tri[1,1]/Tri[0,0]*m6g[1]+Tri[1,0]/Tri[0,0]*m6g[2])*Ts
 phi=np.array([Yaw,x[0]-x0[0],x[1]-x0[1]])
 print(phi)
 time.sleep(Ts-time.time()+t_estimate_Attitude0)

このコードで一応推定した姿勢角をプリントできるはず…　記事用に書き換えたので間違ってるかも…
あと上コードではプロセスノイズの決め方にもひと工夫してます。

上のコードで計算したものをリアルタイムでROSのrvizで可視化しました。動画のリンクを貼っときます。
https://www.youtube.com/watch?time_continue=35&v=c6k5I5ItSg0
ドローンの上部についてる黒いのと緑のやつがRaspberryPi3とNavio2です。とりあえず、見た目的には推定できてるっぽい。一応250Hzで推定してます。

#感想
姿勢表現はプログラムしてるうちに座標がぐちゃぐちゃになりがち。特に座標変換に中途半端にROSのtfライブラリ使ったりしたら…
カルマンフィルタ自体は簡単！