卡尔曼求pitch

#卡尔曼滤波
#求取pitch

import math

Q_angle = 0.001 #陀螺仪噪声的协方差
Q_gyro = 0.005 #陀螺仪漂移噪声的协方差
C_0 = 1
R_angle = 0.5 #加速度计的协方差
q_bias = 0.1 #陀螺仪漂移
dt = 0.005
P = [0, 0, 0, 0]
PP = [[1, 0], [0, 1]]
anglek = 0

def Update_IMU(ax, ay, az, gx, gy, gz):
global anglek, q_bias, P, Pdot
angle_m = math.atan(ax/az)180/math.pi
gyro_m = -gy/16.4
anglek += (gyro_m-q_bias)dt
angle_err = angle_m-anglek
P[0] = Q_angle-PP[0][1]-PP[1][0]
P[1] -= PP[1][1]
P[2] -= PP[1][1]
P[3] = Q_gyro
PP[0][0] += P[0]dt
PP[0][1] += P[1]dt
PP[1][0] += P[2]dt
PP[1][1] += P[3]dt
PCt_0 = C_0PP[0][0]
PCt_1 = C_0PP[1][0]
E = R_angle+C_0PCt_0
K_0 = PCt_0/E
K_1 = PCt_1/E
t_0 = PCt_0
t_1 = C_0PP[0][1]
PP[0][0] -= K_0t_0
PP[0][1] -= K_0t_1
PP[1][0] -= K_1t_0
PP[1][1] -= K_1t_1
anglek += K_0*angle_err
return anglek