KalmanFilter.h卡尔曼滤波头文件+arduino

最新推荐文章于 2025-03-26 09:51:59 发布

USTC暖暖

最新推荐文章于 2025-03-26 09:51:59 发布

阅读量4.6k

点赞数 4

分类专栏： arduino 文章标签： Kalman 卡尔曼滤波 Arduino 头文件 KalmanFilter.h

本文链接：https://blog.csdn.net/USTCsunyue/article/details/88851335

版权

#include "KalmanFilter.h"
//yijielvbo
void KalmanFilter::Yiorderfilter(float angle_m, float gyro_m,float dt,float K1)
{
  angle6 = K1 * angle_m + (1 - K1) * (angle6 + gyro_m * dt);
 // return angle6;
}
kalman/

void KalmanFilter::Kalman_Filter(double angle_m, double gyro_m,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0)
{
  angle += (gyro_m - q_bias) * dt;
  angle_err = angle_m - angle;
  Pdot[0] = Q_angle - P[0][1] - P[1][0];
  Pdot[1] = - P[1][1];
  Pdot[2] = - P[1][1];
  Pdot[3] = Q_gyro;
  P[0][0] += Pdot[0] * dt;
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  E = R_angle + C_0 * PCt_0;
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  t_0 = PCt_0;
  t_1 = C_0 * P[0][1];
  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1