久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

標(biāo)題: C 陀螺儀和加速度的值卡爾曼濾波融合 [打印本頁(yè)]

作者: 51hei小林 時(shí)間: 2016-9-25 10:55
標(biāo)題: C 陀螺儀和加速度的值卡爾曼濾波融合
arduino的卡爾曼濾波融合算法，非原創(chuàng)，我只是封裝了算法。

H文件：
/*
* KalmanFilter.h
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/

#ifndef KalmanFilter_h
#define KalmanFilter_h

#include <WProgram.h>

class KalmanFilter
{
  public:
  KalmanFilter();

  /*
卡爾曼融合計(jì)算
angle_m: 加速度計(jì)測(cè)量并通過(guò)atan2(ax,ay)方法計(jì)算得到的角度（弧度值）
gyro_m:陀螺儀測(cè)量的角速度值（弧度值）
dt:采樣時(shí)間(s)
outAngle:卡爾曼融合計(jì)算出的角度（弧度值）
outAngleDot:卡爾曼融合計(jì)算出的角速度（弧度值）
  */
  void getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot);

  private:
  double C_0, Q_angle, Q_gyro, R_angle;
  double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
  double angle, angle_dot;
  double P[2][2];
  double Pdot[4];
};

CPP文件：
/*
* KalmanFilter.cpp
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter()
{
C_0 = 1.0f;
Q_angle = 0.001f;
Q_gyro = 0.003f;
R_angle = 0.5f;
q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;
angle = angle_dot = 0.0f;
P[0][0] = 1.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 1.0f;
Pdot[0] = 0.0f;
Pdot[1] = 0.0f;
Pdot[2] = 0.0f;
Pdot[3] = 0.0f;
}

void  KalmanFilter::getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot)
{
/*
  Serial.print("angle_m = ");
  Serial.print(angle_m);
  Serial.print(";");
  Serial.print("gyro_m = ");
  Serial.print(gyro_m);
  Serial.print(";");
*/

  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][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  angle += K_0 * angle_err;
  q_bias += K_1 * angle_err;
  angle_dot = gyro_m-q_bias;

  outAngle = angle;
  outAngleDot = angle_dot;
/*
  Serial.print("angle = ");
  Serial.print(angle);
  Serial.print(";");
  Serial.print("angle_dot = ");
  Serial.print(angle_dot);
  Serial.print(";");
*/
}
#endif

作者: yq97723997 時(shí)間: 2016-10-17 22:55
你好非常感謝的你的資料

作者: terry1982 時(shí)間: 2018-1-10 20:31
gyro_m:陀螺儀測(cè)量的角速度值（弧度值）

gyro_m 的值是怎么計(jì)算出來(lái)的？

作者: joyduino 時(shí)間: 2018-5-3 00:25
請(qǐng)教#include <WProgram.h>這行里 WProgram.h是什么作用的文件？
帖子里沒(méi)看到有提到這個(gè)WProgram.h

歡迎光臨 (http://www.zg4o1577.cn/bbs/)

<nav id="cmumg"></nav>