本项目基于瑞芯微芯片,基于Arduion平台,实现了ICM42677(IMU芯片)的数据采集,利用姿态解算和卡尔曼滤波,输出角度。移植了相应的日志系统,便于数据采集
本次基于ICM42688芯片
Arduino平台下的:https://github.com/finani/ICM42688
Kalman Filter算法的详细过程包括两部分,即5个公式:
预测阶段:计算先验估计(P(k|k-1)是X(k|k-1)对应的协方差)
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
修正阶段:计算后验估计并更新先验估计的协方差
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
具体推倒请参考博文:卡尔曼滤波(Kalman filter) 含详细数学推导 - 知乎
1计算角度共有两个计算方法,①通过角速度gyro对时间t的积分,但是存在漂移的静态误差;②加速度计进行姿态解算,但是由于骑行过程中的冲力、动力加速度的等影响存在高频的噪声。因此,仅是利用其中某一种计算方法,都会导致的计算不准确。所以我们采用卡尔曼滤波算法来进行计算。逻辑原理如下图所示:
卡尔曼滤波的相关阈值,“gsensor.h”之中的结构体“KALMAN_FILTER_PARA”之中。
名称 | 阈值解释 |
---|---|
Q_bias | 系统中陀螺仪产生的漂移的过程噪声,可以通过调参获得最优值 |
Q_angle; | 系统中陀螺仪在该时刻产生的角度过程噪声,可以通过调参获得最优值 |
Q_gyro | 统中在roll方向上的陀螺仪产生的角度的过程噪声 |
R_angle | 系统中在roll方向上的陀螺仪产生的角度的测量噪声,用过0.3 |
dt | 卡尔曼滤波过程中计算间隔时间 |
PP[2][2] | 系统的协方差矩阵 |
K_0 | 卡尔曼增益 |
K_1 | 卡尔曼增益 |