卡尔曼滤波

不是我吧12138

已于 2022-09-09 20:11:45 修改

阅读量188

点赞数

文章标签： stm32

于 2022-09-07 18:17:20 首次发布

本文链接：https://blog.csdn.net/HYWSWYQ/article/details/126751335

版权

该博客详细介绍了如何运用卡尔曼滤波算法来处理传感器数据，特别是针对陀螺仪测量的角速度进行优化。通过设置过程噪声和测量噪声的协方差，以及采样时间，博主展示了如何进行先验估计、协方差矩阵更新和卡尔曼增益计算，以获得更准确的角度估计值。

摘要由CSDN通过智能技术生成

#include "filter.h"

float angle;
float Q_angle = 0.001;//过程噪声的协方差
float Q_gyro = 0.003;
float R_angle = 0.03; //测量噪声的协方差
float dt = 0.05; //采样时间 = 1/f
float newgyro;   //陀螺仪测得角速度
float Q_bias,Angle_err;
float K_0, K_1;//卡尔曼增益系数
float P[2][2] = { { 1, 0 },{ 0, 1 } };//协方差矩阵
float measure = 0.0;
/* 系统过程协方差Q定义：
 * |cov(angle , angle)  cov(Q_bias , angle) |
 * |cov(angle , Q_bias) cov(Q_bias , Q_bias)|
 */
void Kalman_Filter(float newAngle, float newGyro){
 //先验估计
 angle = angle - Q_bias * dt + newGyro * dt;  
 //Q_bias = Q_bias;s
 //先验估计协方差
 P[0][0] = P[0][0] + Q_angle - (P[0][1] - P[1][0]) * dt;
 P[0][1] = P[0][1] - P[1][1] * dt;
 P[1][0] = P[1][0] - P[1][1] * dt;
 P[1][1] = P[1][0] + Q_gyro;
 //建立测量方程
 measure = newAngle;
 //计算卡尔曼增益
 K_0 = (P[0][0]) / (P[0][0] + R_angle);
 K_1 = (P[1][0]) / (P[1][0] + R_angle);
 //计算当前最优估计值
 angle = angle + K_0 * (newAngle - angle);
 Q_bias = Q_bias + K_1 * ( newAngle - angle);
 //更新协方差矩阵
 P[0][0] = P[0][0] - K_0 * P[0][0];
 P[0][1] = P[0][1] - K_0 * P[0][1];
 P[1][0] = P[1][0] - K_1 * P[0][0];
 P[1][1] = P[1][1] - K_1 * P[0][1];
}