扩展卡尔曼滤波EKF程序姿态解算陀螺仪角速度小角模式算法非四元数更新算法。 算法原理:利用陀螺仪小角姿态矩阵把加速度地磁数据估计值从上个状态旋转到下个状态这作为时间迭代估计再利用加计地磁实时测量值来修正。 从而达到陀螺和加计地磁融合的目的。 最终通过融合滤波后的加计和地磁直接算姿态角省去了四元数。 在主图所示红色为滤波前绿色为滤波后。 实验表明这种小角更新原理的精度远远超过四元数更新版本程序在stm32f407单片机运行实时传到手机app显示最近在搞姿态解算这块发现了一个超有趣的算法——扩展卡尔曼滤波EKF程序结合陀螺仪角速度小角模式算法而且还是非四元数更新算法哦算法原理揭秘这个算法的原理挺巧妙的。它利用陀螺仪小角姿态矩阵把加速度和地磁数据估计值从上个状态旋转到下个状态这就像是给数据安排了一场奇妙的“时空旅行”作为时间迭代估计。然后呢再利用加计地磁实时测量值来修正。这么一操作就实现了陀螺和加计地磁的融合啦。比如说我们有一个状态向量$x$它包含了姿态等信息。在时间$k$时通过陀螺仪的小角姿态矩阵$Ak$我们可以把上个状态的估计值$x{k-1}$变换到当前时间$k$的估计值$\hat{x}{k|k-1}$即$\hat{x}{k|k-1} Ak x{k-1}$。然后呢再结合加计地磁的实时测量值$z_k$利用扩展卡尔曼滤波的更新公式来修正这个估计值。扩展卡尔曼滤波EKF程序姿态解算陀螺仪角速度小角模式算法非四元数更新算法。 算法原理:利用陀螺仪小角姿态矩阵把加速度地磁数据估计值从上个状态旋转到下个状态这作为时间迭代估计再利用加计地磁实时测量值来修正。 从而达到陀螺和加计地磁融合的目的。 最终通过融合滤波后的加计和地磁直接算姿态角省去了四元数。 在主图所示红色为滤波前绿色为滤波后。 实验表明这种小角更新原理的精度远远超过四元数更新版本程序在stm32f407单片机运行实时传到手机app显示这里简单说下扩展卡尔曼滤波的更新公式。先计算卡尔曼增益$Kk$它的计算涉及到状态估计误差协方差$P{k|k-1}$和测量噪声协方差$Rk$等。公式大概是$Kk P{k|k-1} Hk^T (Hk P{k|k-1} Hk^T Rk)^{-1}$其中$Hk$是观测矩阵。然后更新状态估计值$\hat{x}{k|k} \hat{x}{k|k-1} Kk (zk - Hk \hat{x}{k|k-1})$同时更新状态估计误差协方差$P{k|k} (I - Kk Hk) P_{k|k-1}$。代码实现与分析在stm32f407单片机上运行这个程序时代码实现也有一些要点。// 初始化一些参数 float Q_angle 0.001; float Q_gyro 0.00001; float R_angle 0.01; float P[6][6] { {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1} }; // 姿态更新函数 void update姿态(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float accel_z) { float angle_est[3]; float gyro_rate[3] {gyro_x, gyro_y, gyro_z}; float accel_meas[3] {accel_x, accel_y, accel_z}; // 利用陀螺仪小角姿态矩阵更新角度估计 angle_est[0] gyro_rate[0] * dt; angle_est[1] gyro_rate[1] * dt; angle_est[2] gyro_rate[2] * dt; // 构建观测矩阵H float H[3][6] { {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0} }; // 计算卡尔曼增益K float K[6][3]; float PHT[6][3]; float HPHt_R[3][3]; for(int i 0; i 6; i) { for(int j 0; j 3; j) { PHT[i][j] 0; for(int k 0; k 6; k) { PHT[i][j] P[i][k] * H[j][k]; } } } for(int i 0; i 3; i) { for(int j 0; j 3; j) { HPHt_R[i][j] 0; for(int k 0; k 3; k) { HPHt_R[i][j] H[i][k] * PHT[k][j]; } HPHt_R[i][j] R_angle; } } for(int i 0; i 6; i) { for(int j 0; j 3; j) { K[i][j] 0; for(int k 0; k 6; k) { K[i][j] P[i][k] * H[j][k]; } for(int k 0; k 3; k) { K[i][j] - K[i][j] * HPHt_R[k][j]; } } } // 更新状态估计值 float angle_err[3] {accel_meas[0] - angle_est[0], accel_meas[1] - angle_est[1], accel_meas[2] - angle_est[2]}; for(int i 0; i 3; i) { angle_est[i] K[0][i] * angle_err[0] K[1][i] * angle_err[1] K[2][i] * angle_err[2]; } // 更新状态估计误差协方差 float I_KH[6][6]; for(int i 0; i 6; i) { for(int j 0; j 6; j) { I_KH[i][j] 0; if(i j) { I_KH[i][j] 1; } } } for(int i 0; i 6; i) { for(int j 0; j 6; j) { for(int k 0; k 3; k) { I_KH[i][j] - K[i][k] * H[k][j]; } } } for(int i 0; i 6; i) { for(int j 0; j 6; j) { P[i][j] 0; for(int k 0; k 6; k) { P[i][j] I_KH[i][k] * P[k][j]; } } } // 这里还可以根据新的角度估计值angle_est来更新姿态矩阵等其他相关参数 }在这段代码里我们首先初始化了一些参数比如过程噪声协方差$Q$和测量噪声协方差$R$还有状态估计误差协方差矩阵$P$。在姿态更新函数里先利用陀螺仪的角速度更新角度估计值。然后构建观测矩阵$H$接着通过一系列计算得到卡尔曼增益$K$。利用加速度的测量值和估计值的误差来更新角度估计值最后更新状态估计误差协方差矩阵$P$。实验结果超惊喜通过实验发现这种小角更新原理的精度远远超过四元数更新版本程序运行在stm32f407单片机上还能实时把数据传到手机app显示。就像主图展示的那样红色为滤波前绿色为滤波后滤波后的效果明显更稳定、更准确。总之这个扩展卡尔曼滤波EKF程序结合陀螺仪角速度小角模式算法真的很赞为姿态解算提供了一种高效且精确的方法