devbox
酷酷是懒虫
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Arduino连接两个MPU6050测算姿态角_arduino calman.h库

作者：酷酷是懒虫 | 2024-08-22 03:24:29

赞

踩

arduino calman.h库

Arduino连接两个MPU6050测算姿态角

单个MPU6050 连接

连线

在这里插入图片描述
采用卡尔曼滤波器，对加速度计进行修正。

//连线方法
//MPU-UNO
//VCC-VCC
//GND-GND
//SCL-A5
//SDA-A4
//INT-2 (Optional)

#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12

Kalman.h 库 可以通过IDE自带的库搜索下载。或者git：https://github.com/TKJElectronics/KalmanFilter
Adafruit_MPU6050 IDE自带，如果没自带 用自带的库搜索下载。

完整代码

#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>
const int RESET_PIN = 2; // 定义复位引脚为 D2
Adafruit_MPU6050 mpu;

float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float AccErrorX, AccErrorY, AccErrorZ,GyroErrorX, GyroErrorY, GyroErrorZ = 0;
float AccSumX, AccSumY, AccSumZ,GyroSumX, GyroSumY, GyroSumZ = 0;
int times = 0;
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
unsigned long nLastTime = 0; //上一次读数的时间
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
Kalman kalmanYaw; //Yaw角滤波器
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  // initialize mpu
  if (!mpu.begin()) {
    Serial.println("Failed to start MPU6050");
    while (1) {
      delay(10);
    }
  } else {
    Serial.println("Start MPU6050");
  }
  // display MPU6050 settings
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
  calculate_IMU_error();
  pinMode(2,OUTPUT);
}

void calculate_IMU_error()
{
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  int c = 0;
  int times_c =200;
  while(c<times_c){
    AccX = a.acceleration.x;
    AccY = a.acceleration.y;
    AccZ = a.acceleration.z;
    AccErrorX+=AccX;AccErrorY+=AccY;AccErrorZ+=AccZ;
    GyroX = g.gyro.x;
    GyroY = g.gyro.y;
    GyroZ = g.gyro.z;
    GyroErrorX+=GyroX;GyroErrorY+=GyroY;GyroErrorZ+=GyroZ;
    delay(0.001);c++;
  }
  AccErrorX = AccErrorX/times_c; AccErrorY = AccErrorY/times_c; AccErrorZ = AccErrorZ/times_c;
  GyroErrorX = GyroErrorX/times_c; GyroErrorY = GyroErrorY/times_c; GyroErrorZ = GyroErrorZ/times_c;
}
//均值滤波
void avg()
{
  int d = 0;
  int times_d = 5;
  AccSumX=0;AccSumY=0;AccSumZ=0;
  GyroSumX=0;GyroSumY=0;GyroSumZ=0;
  while(d<times_d)
  {
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    AccX = a.acceleration.x;
    AccY = a.acceleration.y;
    AccZ = a.acceleration.z;
    GyroX = g.gyro.x;
    GyroY = g.gyro.y;
    GyroZ = g.gyro.z;
    AccSumX+=AccX;AccSumY+=AccY;AccSumZ+=AccZ;
    GyroSumX+=GyroX;GyroSumY+=GyroY;GyroSumZ+=GyroZ;
    delay(0.001);d++;
  }
  AccX = AccSumX/times_d+AccErrorX;AccY = AccSumY/times_d+AccErrorY;AccZ = AccSumZ/times_d+AccErrorZ;
  GyroX=GyroSumX/times_d+GyroErrorX;GyroY=GyroSumY/times_d+GyroErrorY;GyroZ=GyroSumZ/times_d+GyroErrorZ;
}
//算得Roll角
float GetRoll(float AccX,float AccZ, float fNorm) {
  float fNormXZ = sqrt(AccX * AccX + AccZ * AccZ);
  float fCos = fNormXZ / fNorm;
  return acos(fCos) * fRad2Deg;
}

//算得Pitch角
float GetPitch(float AccY,float AccZ, float fNorm) {
  float fNormYZ = sqrt(AccY * AccY + AccZ * AccZ);
  float fCos = fNormYZ / fNorm;
  return acos(fCos) * fRad2Deg;
}
//算得yaw角
float GetYaw(float AccX,float AccY, float fNorm) {
  float fNormXY = sqrt(AccX * AccX + AccY * AccY);
  float fCos = fNormXY / fNorm;
  return acos(fCos) * fRad2Deg;
}
void loop() 
{
  //简单均值滤波并修正
  avg();
  //计算加速度向量的模长，均以g为单位
  float fNorm = sqrt(AccX * AccX + AccY * AccY + AccZ * AccZ);
  float fRoll = GetRoll(AccX,AccZ, fNorm); //计算Roll角
  if (AccY > 0) {
    fRoll = -fRoll;
  }
  float fPitch = GetPitch(AccY,AccZ, fNorm); //计算Pitch角
  if (AccX < 0) {
    fPitch = -fPitch;
  }
  float fYaw = GetYaw(AccX,AccY,fNorm);//计算Yaw角
  if(AccZ<0){
    fYaw = -fYaw;
  }
  //计算两次测量的时间间隔dt，以秒为单位
  unsigned long nCurTime = micros();
  float dt = (double)(nCurTime - nLastTime) / 1000000.0;
  //对Roll角和Pitch角进行卡尔曼滤波
  //rad2deg
  float GyroX_deg =GyroX*fRad2Deg;
  float GyroY_deg =GyroY*fRad2Deg;
  float GyroZ_deg =GyroZ*fRad2Deg;
  // 这个*2是在测试的时候发现测量数值是前者一半的时候补充的。理论推导是没有的。
  // 后面在开发两个MPU6050时发现，是开头校准的时候出现的问题，不应该拿角度直接校准。
  // yaw角 六轴陀螺仪测不准 ， 用加速度计测量会不变（这里加入了角速度修正会移动然后回归到原先的值）如果采用角速度测量，加速度修正则会漂移。
  float fNewRoll = kalmanRoll.getAngle(2*fRoll, GyroX_deg, dt);
  float fNewPitch = kalmanPitch.getAngle(2*fPitch, GyroY_deg,dt);
  float fNewYaw = kalmanYaw.getAngle(fYaw, GyroZ_deg,dt);
  // //跟据滤波值计算角度速
  // float fRollRate = (fNewRoll - fLastRoll) / dt;
  // float fPitchRate = (fNewPitch - fLastPitch) / dt;
 
 //更新Roll角和Pitch角
  fLastRoll = fNewRoll;
  fLastPitch = fNewPitch;
  Serial.print(fNewRoll); 
  Serial.print(",");
  Serial.print(fNewPitch);
  Serial.print(",");
  Serial.print(fNewYaw);
  Serial.print("\n");
  //更新本次测的时间
  nLastTime = nCurTime;
   // 检测复位信号
  if (digitalRead(RESET_PIN) == HIGH) {
    // 复位信号为低电平，说明 reset 按钮被按下
    Serial.end(); // 关闭串口通讯
    delay(10000);   // 延时一段时间以确保串口通讯关闭
  }
  
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
备注

yaw角 六轴陀螺仪测不准 ， 用加速度计测量会不变（这里加入了角速度修正会移动然后回归到原先的值）如果采用角速度测量，加速度修正则会漂移。需要换成九轴（磁力计修正）才可以测准。

两个MPU

偷个图

这里AD0 接ACC可以将iic地址变为0x69，接地是0x68 .这样可以实现连接两个陀螺仪的功能。

这里测试`
if (!mpu1.begin(0x68)) 
  {
    Serial.println("Failed to start MPU60501");
    while (1) {
      delay(10);
    }
  } 
  else 
  {
    Serial.println("Start MPU60501");
  }
 
  // initialize mpu
  if (!mpu2.begin(0x69))
  {
    Serial.println("Failed to start MPU60502");
    while (1) {
      delay(10);
    }
  } 
  else
  {
    Serial.println("Start MPU60502");
  } 

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

完整代码

#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>
const int RESET_PIN = 2; // 定义复位引脚为 D2
Adafruit_MPU6050 mpu1;
Adafruit_MPU6050 mpu2;
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
float Acc1[3];
float Gyro1[3];
float AccError1[3];
float GyroError1[3];
float AccSum1[3];
float GyroSum1[3];
float Acc2[3];
float Gyro2[3];
float AccError2[3];
float GyroError2[3];
float AccSum2[3];
float GyroSum2[3];
float deta1Roll;
float deta1Pitch;
// float deta1Yaw;
float deta2Roll;
float deta2Pitch;
// float deta2Yaw;
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
unsigned long nLastTime = 0; //上一次读数的时间
unsigned long nLastTime2 = 0; //上一次读数的时间
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
Kalman kalmanYaw; //Yaw角滤波器
Kalman kalmanRoll2; //Roll角滤波器
Kalman kalmanPitch2; //Pitch角滤波器
Kalman kalmanYaw2; //Yaw角滤波器
void setup(){
  // put your setup code here, to run once:
  Serial.begin(115200);
  // initialize mpu
  if (!mpu1.begin(0x68)) 
  {
    Serial.println("Failed to start MPU60501");
    while (1) {
      delay(10);
    }
  } 
  else 
  {
    Serial.println("Start MPU60501");
  }
 
  // initialize mpu
  if (!mpu2.begin(0x69))
  {
    Serial.println("Failed to start MPU60502");
    while (1) {
      delay(10);
    }
  } 
  else
  {
    Serial.println("Start MPU60502");
  } 
  // display MPU6050 settings
  mpu1.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu1.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu1.setFilterBandwidth(MPU6050_BAND_21_HZ);
  // display MPU6050 settings
  mpu2.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu2.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu2.setFilterBandwidth(MPU6050_BAND_21_HZ);
  // calculate_IMU_error(1);
  // calculate_IMU_error(2);
}
void calculate_IMU_error(int which){
  if (which == 1){
    sensors_event_t a, g, temp;
    mpu1.getEvent(&a, &g, &temp);
    int c = 0;
    int times_c =200;
    while(c<times_c)
    {
      Acc1[0] = a.acceleration.x;
      Acc1[1]= a.acceleration.y;
      Acc1[2] = a.acceleration.z;
      AccError1[0]+=Acc1[0];AccError1[1]+=Acc1[1];AccError1[2]+=Acc1[2];
      Gyro1[0] = g.gyro.x;
      Gyro1[1] = g.gyro.y;
      Gyro1[2] = g.gyro.z;
      GyroError1[0]+=Gyro1[0];GyroError1[1]+=Gyro1[1];GyroError1[2]+=Gyro1[2];
      delay(0.001);c++;
    }
    AccError1[0] = AccError1[0]/times_c; AccError1[1] = AccError1[1]/times_c; AccError1[2] = AccError1[2]/times_c;
    // GyroError1[0] = GyroError1[0]/times_c; GyroError1[1] = GyroError1[1]/times_c; GyroError1[2] = GyroError1[2]/times_c;
    deta1Roll = GetRoll(AccError1[0],AccError1[2],sqrt(AccError1[0] * AccError1[0] + AccError1[1] * AccError1[1] + AccError1[2] * AccError1[2]));
    deta1Pitch = GetPitch(AccError1[1],AccError1[2],sqrt(AccError1[0] * AccError1[0] + AccError1[1] * AccError1[1] + AccError1[2] * AccError1[2]));
  }
  if (which == 2){
   sensors_event_t a, g, temp;
   mpu2.getEvent(&a, &g, &temp);
   int c = 0;
   int times_c =200;
    while(c<times_c)
    {
      Acc2[0] = a.acceleration.x;
      Acc2[1]= a.acceleration.y;
      Acc2[2] = a.acceleration.z;
      AccError2[0]+=Acc2[0];AccError2[1]+=Acc2[1];AccError2[2]+=Acc2[2];
      Gyro2[0] = g.gyro.x;
      Gyro2[1] = g.gyro.y;
      Gyro2[2] = g.gyro.z;
      GyroError2[0]+=Gyro2[0];GyroError2[1]+=Gyro2[1];GyroError2[2]+=Gyro2[2];
      delay(0.001);c++;
    }
    AccError2[0] = AccError2[0]/times_c; AccError2[1] = AccError2[1]/times_c; AccError2[2] = AccError2[2]/times_c;
    // GyroError2[0] = GyroError2[0]/times_c; GyroError2[1] = GyroError2[1]/times_c; GyroError2[2] = GyroError2[2]/times_c;
    deta2Roll = GetRoll(AccError2[0],AccError2[2],sqrt(AccError2[0] * AccError2[0] + AccError2[1] * AccError2[1] + AccError2[2] * AccError2[2]));
    deta2Pitch = GetPitch(AccError2[1],AccError2[2],sqrt(AccError2[0] * AccError2[0] + AccError2[1] * AccError2[1] + AccError2[2] * AccError2[2]));
  }
 
}
void avg(int which){
  int d = 0;
  int times_d = 5;
  if(which==1)
  {
    AccSum1[0]=0;AccSum1[1]=0;AccSum1[2]=0;
    GyroSum1[0]=0;GyroSum1[1]=0;GyroSum1[2]=0;
    while(d<times_d)
    {
      sensors_event_t a, g, temp;
      mpu1.getEvent(&a, &g, &temp);
      Acc1[0] = a.acceleration.x;
      Acc1[1] = a.acceleration.y;
      Acc1[2] = a.acceleration.z;
      Gyro1[0] = g.gyro.x;
      Gyro1[1] = g.gyro.y;
      Gyro1[2] = g.gyro.z;
      AccSum1[0]+=Acc1[0];AccSum1[1]+=Acc1[1];AccSum1[2]+=Acc1[2];
      GyroSum1[0]+=Gyro1[0];GyroSum1[1]+=Gyro1[1];GyroSum1[2]+=Gyro1[2];
      delay(0.001);d++;
    }
      // Acc1[0] = AccSum1[0]/times_d+AccError1[0];Acc1[1] = AccSum1[1]/times_d+AccError1[1];Acc1[2] = AccSum1[2]/times_d+AccError1[2];
      // Gyro1[0]=GyroSum1[0]/times_d+GyroError1[0];Gyro1[1]=GyroSum1[1]/times_d+GyroError1[1];Gyro1[2]=GyroSum1[2]/times_d+GyroError1[2];
      Acc1[0] = AccSum1[0]/times_d;Acc1[1] = AccSum1[1]/times_d;Acc1[2] = AccSum1[2]/times_d;
      Gyro1[0]=GyroSum1[0]/times_d;Gyro1[1]=GyroSum1[1]/times_d;Gyro1[2]=GyroSum1[2]/times_d;

  }
  if(which==2)
  {
    AccSum2[0]=0;AccSum2[1]=0;AccSum2[2]=0;
    GyroSum2[0]=0;GyroSum2[1]=0;GyroSum2[2]=0;
    while(d<times_d)
    {
      sensors_event_t a, g, temp;
      mpu2.getEvent(&a, &g, &temp);
      Acc2[0] = a.acceleration.x;
      Acc2[1] = a.acceleration.y;
      Acc2[2] = a.acceleration.z;
      Gyro2[0] = g.gyro.x;
      Gyro2[1] = g.gyro.y;
      Gyro2[2] = g.gyro.z;
      AccSum2[0]+=Acc2[0];AccSum2[1]+=Acc2[1];AccSum2[2]+=Acc2[2];
      GyroSum2[0]+=Gyro2[0];GyroSum2[1]+=Gyro2[1];GyroSum2[2]+=Gyro2[2];
      delay(0.001);d++;
    }
      // Acc2[0] = AccSum2[0]/times_d+AccError2[0];Acc2[1] = AccSum2[1]/times_d+AccError2[1];Acc2[2] = AccSum2[2]/times_d+AccError2[2];
      // Gyro2[0]=GyroSum2[0]/times_d+GyroError2[0];Gyro2[1]=GyroSum2[1]/times_d+GyroError2[1];Gyro2[2]=GyroSum2[2]/times_d+GyroError2[2];
      Acc2[0] = AccSum2[0]/times_d;Acc2[1] = AccSum2[1]/times_d;Acc2[2] = AccSum2[2]/times_d;
      Gyro2[0]=GyroSum2[0]/times_d;Gyro2[1]=GyroSum2[1]/times_d;Gyro2[2]=GyroSum2[2]/times_d;
  }
}
//算得Roll角
float GetRoll(float AccX,float AccZ, float fNorm) {
  float fNormXZ = sqrt(AccX * AccX + AccZ * AccZ);
  float fCos = fNormXZ / fNorm;
  return acos(fCos) * fRad2Deg;
}

//算得Pitch角
float GetPitch(float AccY,float AccZ, float fNorm) {
  float fNormYZ = sqrt(AccY * AccY + AccZ * AccZ);
  float fCos = fNormYZ / fNorm;
  return acos(fCos) * fRad2Deg;
}
//算得yaw角
float GetYaw(float AccX,float AccY, float fNorm) {
  float fNormXY = sqrt(AccX * AccX + AccY * AccY);
  float fCos = fNormXY / fNorm;
  return acos(fCos) * fRad2Deg;
}
void loop() {
  avg(1);
  //计算加速度向量的模长，均以g为单位
  float fNorm = sqrt(Acc1[0] * Acc1[0] + Acc1[1] * Acc1[1] + Acc1[2] * Acc1[2]);
  float fRoll = GetRoll(Acc1[0],Acc1[2], fNorm)+deta1Roll; //计算Roll角
  if (Acc1[1] > 0) {
    fRoll = -fRoll;
  }
  float fPitch = GetPitch(Acc1[1],Acc1[2], fNorm)+deta1Pitch; //计算Pitch角
  if (Acc1[0] < 0) {
    fPitch = -fPitch;
  }
  float fYaw = GetYaw(Acc1[0],Acc1[1],fNorm);//计算Yaw角
  if(Acc1[2]<0){
    fYaw = -fYaw;
  }
  //计算两次测量的时间间隔dt，以秒为单位
  unsigned long nCurTime = micros();
  float dt = (double)(nCurTime - nLastTime) / 1000000.0;
  //对Roll角和Pitch角进行卡尔曼滤波
  //rad2deg
  float GyroX_deg =Gyro1[0]*fRad2Deg;
  float GyroY_deg =Gyro1[1]*fRad2Deg;
  float GyroZ_deg =Gyro1[2]*fRad2Deg;
  /// 将单MPU的问题修正
  float fNewRoll = kalmanRoll.getAngle(fRoll, GyroX_deg, dt);
  float fNewPitch = kalmanPitch.getAngle(fPitch, GyroY_deg,dt);
  float fNewYaw = kalmanYaw.getAngle(fYaw, GyroZ_deg,dt);
 
  Serial.print(fNewRoll); 
  Serial.print(",");
  Serial.print(fNewPitch);
  Serial.print(",");
  Serial.print(fNewYaw);
 //更新本次测的时间
  nLastTime = nCurTime;
  //mpu2//下面没有定义直接调用
  avg(2);
  //计算加速度向量的模长，均以g为单位
  float fNorm2 = sqrt(Acc2[0] * Acc2[0] + Acc2[1] * Acc2[1] + Acc2[2] * Acc2[2]);
  float fRoll2 = GetRoll(Acc2[0],Acc2[2], fNorm2)+deta2Roll; //计算Roll角
  if (Acc2[1] > 0) {
    fRoll2 = -fRoll2;
  }
  float fPitch2 = GetPitch(Acc2[1],Acc2[2], fNorm2)+deta2Pitch; //计算Pitch角
  if (Acc2[0] < 0) {
    fPitch2 = -fPitch2;
  }
  float fYaw2 = GetYaw(Acc2[0],Acc2[1],fNorm2);//计算Yaw角
  if(Acc2[2]<0){
    fYaw2 = -fYaw2;
  }
  //计算两次测量的时间间隔dt，以秒为单位
  float nCurTime2 = micros();
  float dt2 = (double)(nCurTime2 - nLastTime2) / 1000000.0;
  //对Roll角和Pitch角进行卡尔曼滤波
  //rad2deg
  float GyroX_deg2 =Gyro2[0]*fRad2Deg;
  float GyroY_deg2 =Gyro2[1]*fRad2Deg;
  float GyroZ_deg2 =Gyro2[2]*fRad2Deg;
  float fNewRoll2 = kalmanRoll2.getAngle(fRoll2, GyroX_deg2, dt2);
  float fNewPitch2 = kalmanPitch2.getAngle(fPitch2, GyroY_deg2,dt2);
  float fNewYaw2 = kalmanYaw2.getAngle(fYaw2, GyroZ_deg2,dt2);
 

  Serial.print(",");
  Serial.print(fNewRoll2); 
  Serial.print(",");
  Serial.print(fNewPitch2);
  Serial.print(",");
  Serial.print(fNewYaw2);
  Serial.print("\n");
  //更新本次测的时间
  nLastTime2 = nCurTime2;
 
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268

实验结果

一号陀螺仪的roll pitch yaw 二号陀螺仪的 roll pitch row

卡尔曼公式推导

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/酷酷是懒虫/article/detail/1014532
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号