卡尔曼滤波

#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];
}