卡尔曼求roll

卡尔曼滤波求取roll的代码实现

最新推荐文章于 2022-06-07 18:49:01 发布

原创最新推荐文章于 2022-06-07 18:49:01 发布 · 261 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#哈

日记专栏收录该内容

25 篇文章

订阅专栏

博客展示了使用卡尔曼滤波求取roll的代码。定义了相关参数，如陀螺仪噪声协方差、加速度计协方差等，还定义了Update_IMU函数，通过输入加速度计和陀螺仪数据，经过一系列计算更新角度值并返回。

#卡尔曼滤波
#求取roll

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(ay/az)180/math.pi
gyro_m = -gx/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