matlabで書かれたコードをpythonで実装したい

前提・実現したいこと

ニンテンドースイッチのjoyconのジャイロセンサーと加速度センサーからカルマンフィルタを使って姿勢推定をしたいです。
matlabで書かれたこのサイトを参考にしてpythonで実装しようとしましたところ、下の画像のようなエラーが発生しました。

発生している問題・エラーメッセージ

only size-1 arrays can be converted to Python scalars

該当のソースコード

python
1
2import numpy as np
3import math
4import time
5import matplotlib.pyplot as plt
6
7joyconからの加速度信号 = [0.1,0.2,0.3]
8joyconからのジャイロ信号 = [0.4,0.5,0.6]
9timebox = np.array((),float)
10startTime = time.perf_counter()
11
12
13# 描画領域を取得
14fig, ax2 = plt.subplots(1, 1)
15
16# y軸方向の描画幅を指定
17ax2.set_ylim((0, 4))
18
19#カルマンフィルタ初期化
20roll = 0.0
21pitch = 0.0
22yaw = 0.0
23dt = 0.1
24xEst = [roll,pitch]
25PEst = np.eye(2)*0.0174*dt**2
26Q = np.eye(2)*0.0174*dt**2
27R = np.eye(2)*dt**2
28H = np.eye(2)
29
30timeNow = time.perf_counter() -startTime
31timebox = np.append(timebox,timeNow)
32
33while True:
34
35
36   
37    timebox = np.append(timebox,timeNow)
38   
39   
40    #データを受信
41    rmsg = joyconからの加速度信号
42
43    #加速度から角度を計算（観測値）
44    accel = joyconからの加速度信号
45    ax = accel[0]
46    ay = accel[1]
47    az = accel[2]
48    rollAcc = math.atan2(ay,az)
49    pitchAcc = -math.atan2(ax,math.sqrt(ay**2+az**2))
50    y = [rollAcc,pitchAcc]
51  
52  
53
54    #ジャイロから角度を計算（予測値）
55    gyro = joyconからのジャイロ信号
56    gx = gyro[0]
57    gy = gyro[1]
58    gz = gyro[2]
59
60    print(roll)
61    droll = gx + math.sin(roll)*math.tan(pitch)*gy + math.cos(roll)*math.tan(pitch)*gz
62    dpitch = math.cos(roll)*gy - math.sin(roll)*gz
63    dyaw = math.sin(roll)/math.cos(pitch)*gy + math.cos(roll)/math.cos(pitch)*gz
64    rollGyro = roll + droll*dt
65    pitchGyro = pitch + dpitch*dt
66    yawGyro = yaw + dyaw*dt
67    xPred = [rollGyro,pitchGyro]
68  
69  
70    
71   #ヤコビ行列の計算
72    F = np.eye(2)
73    F[0,0] = 1 + (gy*math.cos(roll)*math.tan(pitch)-gz*math.sin(roll)*math.tan(pitch))*dt
74    F[0,1] = (gy*math.sin(roll)/(math.cos(pitch)**2)+gz*math.cos(roll)/(math.cos(pitch)**2))*dt
75    F[1,0] = (-gy*math.sin(roll)+gz*math.cos(roll))*dt
76    F[1,1] = 1
77  
78    #更新
79    PPred = F*PEst*F.T+Q
80    K = PPred* H.T/(H*PPred*H.T+R)
81    xEst = xPred + K*(y-H*xPred)
82    PEst = (np.eye(2)-K*H)*PPred
83    roll = xEst[0]
84    pitch = xEst[1]
85    yaw = yawGyro
86    
87    
88    
89    # グラフを描画する
90    line, = ax2.plot(timebox,roll,label="roll", color='blue')
91    line2, = ax2.plot(timebox, pitch, color='red')
92    
93    #グラフX軸更新
94    
95    ax2.set_xlim((-10 + timeNow, 10 +timeNow))
96
97    # 次の描画まで0.01秒待つ
98    plt.pause(0.01)
99    # グラフをクリア
100    line.remove()
101    line2.remove()
102

原因と予想すること

matlab
1% 更新
2    PPred = F*PEst*F'+Q;
3    K = PPred*H'/(H*PPred*H'+R);
4    xEst = xPred + K*(y-H*xPred);
5    PEst = (eye(2)-K*H)*PPred;
6
7    roll = xEst(1);
8    pitch = xEst(2);
9    yaw = yawGyro;

matlabで書かれた
xEst(1)
に対する値は数字が返ってくると思うのですが

python
1 #更新
2    PPred = F*PEst*F.T+Q
3    K = PPred* H.T/(H*PPred*H.T+R)
4    xEst = xPred + K*(y-H*xPred)
5    PEst = (np.eye(2)-K*H)*PPred
6    roll = xEst[0]
7    pitch = xEst[1]
8    yaw = yawGyro