カルマンフィルタで角度推定 (加速度センサ＋ジャイロセンサ)

以前購入していた加速度センサ、ジャイロセンサの使い方を思い出す為に、それぞれの値を取得するプログラムを書きました。それだけでは面白くないので、カルマンフィルタで値を統合してみました。

機器構成はArduino nanoに加速度センサ(MMA7361)、ジャイロセンサ(ENC-03R)を接続し、更に、ArduinoとRaspberry piを繋いでシリアル通信でデータのやり取りをする、というものです。Arduinoを使う理由はアナログ入力が楽だから、Raspberry piを使う理由はカルマンフィルタを使うのにpythonでプログラムを書きたかったからです。
なお、センサは秋月電子で買える以下のものです。

・３軸加速度センサモジュール　ＭＭＡ７３６１（アナログ出力）
・小型圧電振動ジャイロモジュール

各センサの接続先は加速度センサのx,y,zがそれぞれArduinoのA0,A1,A2、ジャイロセンサのOUT1がA3,OUT2がA4です。
加速度センサは計測範囲を広げるため、gSに3.3V入力します。

また、事前準備として、加速度センサを使うためのライブラリをインストールしておきます。

github.com

Arduino側プログラム

#include <AcceleroMMA7361.h>
#include <stdio.h>

AcceleroMMA7361 accelero;

int ax=0;
int ay=0;
int az=0;

int wx=0;
int wy=0;

char buf[50];

void setup(){
 Serial.begin(9600);
 accelero.begin(13,12,11,10,A0,A1,A2);
 accelero.setARefVoltage(3.3);
 accelero.setSensitivity(LOW);
 accelero.calibrate();
}

void loop(){
 ax=accelero.getXAccel();
 ay=accelero.getYAccel();
 az=accelero.getZAccel();
 
 wx=analogRead(3);
 wy=analogRead(4);
 
 //ax=accelero.getXRaw();
 //ay=accelero.getYRaw();
 //az=accelero.getZRaw();
 
 sprintf(buf,"%d,%d,%d,%d,%d\n",ax,ay,az,wx,wy);
 
 Serial.write(buf);
 delay(1);
}

Raspberry pi側プログラム

import serial
import numpy as np
import time

ser=serial.Serial("/dev/ttyUSB0",9600)

data=np.zeros(5)

#offset=1.35/3.3*1023
offset=np.array((445,442))

F=np.matrix(np.identity(2))
H=np.matrix(np.identity(2))
x=np.zeros((2,1))
y=np.zeros((2,1))
u=np.zeros((2,1))
dt=0.01
P=np.zeros((2,2))
Q=np.array(((1E-3,0),(0,1E-3)))
R=np.array(((1E-2,0),(0,1E-2)))
t=time.time()
a=np.zeros((2,1))

elapsedTime=0
w=np.zeros((2,1))
rawData=[]
estData=[]
t=time.time()
dt=0

firstFlag=True

while True:
    val=str(ser.readline())
    temp=val.split(",")

    try:
        for i in range(5):
            data[i]=float(temp[i])

        print (data[0],data[1],data[2],data[3],data[4])

        #For initializing time
        if (firstFlag):
            firstFlag=False
            t=time.time()

        #angular acceleration (gyro sensor)
        a[0]=(data[3]-offset[0])/1023*3.3/0.00067*np.pi/180
        a[1]=(data[4]-offset[1])/1023*3.3/0.00067*np.pi/180

        #angular velocity (gyro sensor)
        for i in range(2):
            if (np.abs(a[i])>5):
                u[i]+=a[i]*dt

        #angle (acceleration sensor)
        y[0]=np.arctan2(data[1],np.sqrt(data[0]**2+data[2]**2))
        y[1]=np.arctan2(data[0],data[2])

        #angle (gyro sensor)
        w+=u*dt*180/np.pi

        #kalman filter
        x=x+u*dt
        P=np.dot(np.dot(F,P),F.T)+Q
        K=np.dot(np.dot(P,H.T),np.linalg.inv(np.dot(np.dot(H,P),H.T)+R))
        x=x+np.dot(K,(y-np.dot(H,x)))
        P=P-np.dot(np.dot(K,H),P)

        estData.append(np.r_[x*180/np.pi,y*180/np.pi,w])
        rawData.append(data)

        dt=time.time()-t
        t=time.time()
        elapsedTime+=dt

        print (int(elapsedTime),x*180/np.pi)

        if (elapsedTime>10):
            np.savetxt("estData.csv",estData,delimiter=",")
            np.savetxt("rawData.csv",rawData,delimiter=",")
            break

    except Exception as e:
        print (e.message)

一応コメントを書いてありますが、xがカルマンフィルタによる推定値、yが加速度による推定値、wがジャイロによる推定値(積分値)です。
QやRの共分散行列の値は適当です。数値を変えてもあまり変わりませんでした。
なお、はじめにプログラムを書いた時、ジャイロによる推定値が明らかにおかしかったので、計算方法が間違っていたのかと思いましたが、 ネットで調べてみると、回路上にハイパスフィルタが入っていて、出力値が角加速度になっているようです。その為、2階積分になるようなプログラムにしています。
また、ジャイロの積分誤差が大きい為、一定値以下の数値は無視しています。

一通り接続してセンサを傾けてみた結果が下記のグラフです。
ジャイロによる値は誤差が大きいですが、加速度による推定値とカルマンフィルタによる推定値は殆ど同じです。
今回の場合だとジャイロセンサがあまり役に立っていない気もしますが、とりあえず傾きが取得できるようになりました。