Python pykalman使用によるセンサーノイズの除去

環境

シングルボード：Raspberry Pi 3 B
言語：python
OS：raspbian buster
IMU：MPU-6050

やりたいこと・現状

Raspberry Pi に IMU を接続し、傾角情報を取得したいです。
実際に傾角(ピッチ角とロール角)の取得はできていますが、センサーからのノイズがひどく、正確なデータの取得が出来ていません。
調べてみますと、普通はカルマンフィルタやカルマン平滑化等を行うようです。
また、「pykalman」というパッケージの存在を知りましたが、調べてみてもいまいち使い方が理解できません。

**
最終的には、PyQtで作成したGUI上に正確な傾角データを出力したいです。
**

わからないこと

・pykalmanについて
１．pykalmanのパッケージを使用することで、取得傾角のノイズ緩和はできるのでしょうか？
２．pykalmanを使用する際の返値(？)は何を入れているのでしょうか？
[参考にさせていただいたサイト様のURL、コード]
URL:https://org-technology.com/posts/pykalman.html

python
1import numpy as np
2from matplotlib import pylab as plt
3from pykalman import KalmanFilter
4
5x = -0.37727
6observations = np.random.normal(x, 0.1, size=50)
7
8kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]),
9                  transition_covariance=0.0000001*np.eye(2))
10
11smoothed_states_pred = kf.em(observations).smooth(observations)[0]
12filtered_states_pred = kf.em(observations).filter(observations)[0]
13
14plt.figure(figsize=(12, 9))
15plt.plot(observations, "-xc", label="observations")
16plt.plot(smoothed_states_pred[:, 0], "b", label="smoothed")
17plt.plot(filtered_states_pred[:, 0], "r", label="filtered")
18plt.axhline(x, color="k", label="truth value")
19plt.legend()
20plt.show()

現状のコード

GUI作成

python
1import sys
2from PyQt5.QtWidgets import *
3from PyQt5.QtGui import *
4from PyQt5.QtCore import *
5import subprocess
6import time
7import numpy as np
8import cv2
9import RPi.GPIO as GPIO
10import MIU
11
12import smbus
13import math
14from time import sleep
15
16
17class Tab1Widget(QWidget):
18
19    def __init__(self):
20        super().__init__() 
21        self.title = "MIUテスト"
22        self.left = 50
23        self.top = 50
24        self.width = 1300
25        self.height = 1200
26        self.initUI()
27        self.counter = 0
28
29    def initUI(self):
30        self.a=0
31
32        btn1 = QPushButton("スタート", self)
33        btn2 = QPushButton("ストップ", self)
34
35        btn1.clicked.connect(self.start )
36        btn2.clicked.connect(self.stop )
37
38        self.textbox3 = QLineEdit(self)
39        self.textbox4 = QLineEdit(self)
40        self.textbox5 = QLineEdit(self)
41
42
43        label2 = QLabel("センサ")
44        label3 = QLabel("ピッチ角")
45        label4 = QLabel("ロール角")
46
47
48        layoutA = QGridLayout()
49        layoutA.addWidget(btn1,0,0)
50        layoutA.addWidget(btn2,0,1)
51        layoutA.addWidget(label3,1,0)
52        layoutA.addWidget(self.textbox3,1,1)
53        layoutA.addWidget(label4,2,0)
54        layoutA.addWidget(self.textbox4,2,1)
55
56
57
58        layoutB = QVBoxLayout()
59        layoutB.addWidget(label2)
60        layoutB.addLayout(layoutA)
61
62
63        self.setLayout(layoutB)
64
65        self.show()
66
67
68    def start(self):
69
70        self.a = 1
71
72
73        self.startMIU()
74
75        timer = QTimer(self)
76        timer.timeout.connect(self.startMIU)
77        timer.start(100)
78
79
80    def startMIU(self):
81        if self.a == 0:
82            return
83
84        self.Pitch, self.Roll = MIU.Sensor(self.Pitch, self.Roll)
85
86
87        pitch = str(round(Pitch))
88        self.textbox3.setText(pitch + "°")
89
90        roll = str(round(Roll))
91        self.textbox4.setText(roll + "°")
92
93    def stop(self):
94        self.a = 0
95        GPIO.clean()
96        print("CLEAN")
97
98if __name__ == "__main__":
99    app = QApplication(sys.argv)
100    ex = Tab1Widget()
101    sys.exit(app.exec_())

傾角取得(MIU.py)

python
1import smbus
2import math
3from time import sleep
4
5DEV_ADDR = 0x68
6
7ACCEL_XOUT = 0x3b
8ACCEL_YOUT = 0x3d
9ACCEL_ZOUT = 0x3f
10TEMP_OUT = 0x41
11GYRO_XOUT = 0x43
12GYRO_YOUT = 0x45
13GYRO_ZOUT = 0x47
14
15PWR_MGMT_1 = 0x6b
16PWR_MGMT_2 = 0x6c
17
18bus = smbus.SMBus(1)
19bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0)
20
21
22def read_word(adr):
23    high = bus.read_byte_data(DEV_ADDR, adr)
24    low = bus.read_byte_data(DEV_ADDR, adr+1)
25    val = (high << 8) + low
26    return val
27
28# Sensor data read
29def read_word_sensor(adr):
30    val = read_word(adr)
31    if (val >= 0x8000):         # minus
32        return -((65535 - val) + 1)
33    else:                       # plus
34        return val
35
36
37def get_temp():
38    temp = read_word_sensor(TEMP_OUT)
39    x = temp / 340 + 36.53      # data sheet(register map)記載の計算式.
40    return x
41
42
43def getGyro():
44    x = read_word_sensor(GYRO_XOUT)/ 131.0
45    y = read_word_sensor(GYRO_YOUT)/ 131.0
46    z = read_word_sensor(GYRO_ZOUT)/ 131.0
47    return [x, y, z]
48
49
50def getAccel():
51    x = read_word_sensor(ACCEL_XOUT)/ 16384.0
52    y= read_word_sensor(ACCEL_YOUT)/ 16384.0
53    z= read_word_sensor(ACCEL_ZOUT)/ 16384.0
54    return [x, y, z]
55
56
57
58
59def Sensor(Roll, Pitch):
60    ax, ay, az = getAccel()
61    gx, gy, gz = getGyro()
62
63    #print ('{0:4.3f},   {0:4.3f},    {0:4.3f},     {0:4.3f},      {0:4.3f},      {0:4.3f},' .format(gx, gy, gz, ax, ay, az))
64    Roll = math.atan(ay/az) * 57.324 #57.324 = 360/2π であり、rad から ° に変換
65    Pitch = math.atan(-ax / math.sqrt( ay* ay+ az*az ) ) * 57.324
66
67    #pitch = math.atan(-ax / (ay*math.sin(roll) + az*math.cos(roll)))
68
69    #print('{0:4.3f},   {0:4.3f},' .format(pitch, roll))
70
71    return Pitch, Roll