├── Kalman.h ├── Kalman.cpp └── Kalman_MPU6050.ino /Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h_ 19 | #define _Kalman_h_ 20 | 21 | class Kalman { 22 | public: 23 | Kalman(); 24 | 25 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 26 | float getAngle(float newAngle, float newRate, float dt); 27 | 28 | void setAngle(float angle); // Used to set angle, this should be set as the starting angle 29 | float getRate(); // Return the unbiased rate 30 | 31 | /* These are used to tune the Kalman filter */ 32 | void setQangle(float Q_angle); 33 | /** 34 | * setQbias(float Q_bias) 35 | * Default value (0.003f) is in Kalman.cpp. 36 | * Raise this to follow input more closely, 37 | * lower this to smooth result of kalman filter. 38 | */ 39 | void setQbias(float Q_bias); 40 | void setRmeasure(float R_measure); 41 | 42 | float getQangle(); 43 | float getQbias(); 44 | float getRmeasure(); 45 | 46 | private: 47 | /* Kalman filter variables */ 48 | float Q_angle; // Process noise variance for the accelerometer 49 | float Q_bias; // Process noise variance for the gyro bias 50 | float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 51 | 52 | float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 53 | float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 54 | float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 55 | 56 | float P[2][2]; // Error covariance matrix - This is a 2x2 matrix 57 | }; 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /Kalman.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #include "Kalman.h" 19 | 20 | Kalman::Kalman() { 21 | /* We will set the variables like so, these can also be tuned by the user */ 22 | Q_angle = 0.001f; 23 | Q_bias = 0.003f; 24 | R_measure = 0.03f; 25 | 26 | angle = 0.0f; // Reset the angle 27 | bias = 0.0f; // Reset bias 28 | 29 | P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 30 | P[0][1] = 0.0f; 31 | P[1][0] = 0.0f; 32 | P[1][1] = 0.0f; 33 | }; 34 | 35 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 36 | float Kalman::getAngle(float newAngle, float newRate, float dt) { 37 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 38 | // Modified by Kristian Lauszus 39 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 40 | 41 | // Discrete Kalman filter time update equations - Time Update ("Predict") 42 | // Update xhat - Project the state ahead 43 | /* Step 1 */ 44 | rate = newRate - bias; 45 | angle += dt * rate; 46 | 47 | // Update estimation error covariance - Project the error covariance ahead 48 | /* Step 2 */ 49 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 50 | P[0][1] -= dt * P[1][1]; 51 | P[1][0] -= dt * P[1][1]; 52 | P[1][1] += Q_bias * dt; 53 | 54 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 55 | // Calculate Kalman gain - Compute the Kalman gain 56 | /* Step 4 */ 57 | float S = P[0][0] + R_measure; // Estimate error 58 | /* Step 5 */ 59 | float K[2]; // Kalman gain - This is a 2x1 vector 60 | K[0] = P[0][0] / S; 61 | K[1] = P[1][0] / S; 62 | 63 | // Calculate angle and bias - Update estimate with measurement zk (newAngle) 64 | /* Step 3 */ 65 | float y = newAngle - angle; // Angle difference 66 | /* Step 6 */ 67 | angle += K[0] * y; 68 | bias += K[1] * y; 69 | 70 | // Calculate estimation error covariance - Update the error covariance 71 | /* Step 7 */ 72 | float P00_temp = P[0][0]; 73 | float P01_temp = P[0][1]; 74 | 75 | P[0][0] -= K[0] * P00_temp; 76 | P[0][1] -= K[0] * P01_temp; 77 | P[1][0] -= K[1] * P00_temp; 78 | P[1][1] -= K[1] * P01_temp; 79 | 80 | return angle; 81 | }; 82 | 83 | void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle 84 | float Kalman::getRate() { return this->rate; }; // Return the unbiased rate 85 | 86 | /* These are used to tune the Kalman filter */ 87 | void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; }; 88 | void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; }; 89 | void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; }; 90 | 91 | float Kalman::getQangle() { return this->Q_angle; }; 92 | float Kalman::getQbias() { return this->Q_bias; }; 93 | float Kalman::getRmeasure() { return this->R_measure; }; 94 | -------------------------------------------------------------------------------- /Kalman_MPU6050.ino: -------------------------------------------------------------------------------- 1 | /******加速度计无法计算偏航角，陀螺仪计算偏航有漂移，需与磁力计融合******/ 2 | #include 3 | #include 4 | #define RESTRICT_PITCH 5 | #define fRad2Deg 57.295779513f //将弧度转为角度的乘数 6 | #define fDeg2Rad 0.0174532925f 7 | #define MPU 0x68 //MPU-6050的I2C地址 8 | #define nValCnt 7 //一次读取寄存器的数量 9 | 10 | #define HMC 0x1E //001 1110b(0x3C>>1), HMC5883的7位i2c地址 11 | #define MagnetcDeclination 0.0698131701f //所在地磁偏角，根据情况自行百度 12 | #define CalThreshold 2 //最大和最小值超过此值，则计算偏移值 13 | float offsetX,offsetY,offsetZ; //磁场偏移 14 | float kalAngleX, kalAngleY,kalAngleZ; //横滚、俯仰、偏航角的卡尔曼融合值 15 | float Gryoyaw; //偏航角的地磁测量值 16 | Kalman kalmanX; // 实例化卡尔曼滤波 17 | Kalman kalmanY; 18 | //Kalman kalmanZ; 19 | long timer; 20 | 21 | void setup() { 22 | Serial.begin(9600); //初始化串口，指定波特率 23 | Wire.begin(); //初始化Wire库 24 | WriteMPUReg(0x6B, 0); //启动MPU6050 25 | //WriteMPUReg(0x6A, ReadMPUReg(0x6A)&0xDF); 26 | WriteMPUReg(0x37, ReadMPUReg(0x37)|0x02); //开启mpu6050的IIC直通，连接磁场传感器 27 | float realVals[nValCnt]; 28 | for(int i=0;i<500;i++)ReadAccGyr(realVals); //读出测量值 29 | float roll,pitch; 30 | GetRollPitch(realVals,&roll,&pitch); 31 | roll *= fRad2Deg; pitch *= fRad2Deg; 32 | kalmanX.setAngle(roll); // 设置初始角 33 | kalmanY.setAngle(pitch); 34 | 35 | //设置HMC5883模式 36 | Wire.beginTransmission(HMC); //开始通信 37 | Wire.write(0x00); //选择配置寄存器A 38 | Wire.write(0x70); //0111 0000b，具体配置见数据手册 39 | Wire.endTransmission(); 40 | Wire.beginTransmission(HMC); 41 | Wire.write(0x02); //选择模式寄存器 42 | Wire.write(0x00); //连续测量模式:0x00,单一测量模式:0x01 43 | Wire.endTransmission(); 44 | calibrateMag(); //地磁计校准 45 | // int x,y,z; 46 | // getRawData(&x,&y,&z); //获取地磁数据 47 | // kalmanZ.setAngle(calculateYaw(pitch,roll,x,y,z) * fRad2Deg); //设定卡尔曼滤波初始值 48 | timer = micros(); //计时 49 | } 50 | 51 | void loop() { 52 | float realVals[nValCnt]; 53 | ReadAccGyr(realVals); //读出测量值 54 | int x,y,z; 55 | getRawData(&x,&y,&z);//获取地磁数据 56 | 57 | double dt = (double)(micros() - timer) / 1000000; // 计算时间差 58 | timer = micros(); //更新时间 59 | 60 | float roll,pitch; 61 | GetRollPitch(realVals,&roll,&pitch); 62 | float Geoyaw = calculateYaw(pitch,roll,x,y,z); 63 | float gyroXrate = realVals[4] / 131.0; // 转换到角度/秒 64 | float gyroYrate = realVals[5] / 131.0; 65 | float gyroZrate = realVals[6] / 131.0; 66 | Gryoyaw += gyroZrate * dt; //由陀螺仪获取偏航角 67 | if(Gryoyaw>360)Gryoyaw=0; //大于360则变为0 68 | // 解决加速度计角度在-180度和180度之间跳跃时的过渡问题 69 | roll *= fRad2Deg; pitch *= fRad2Deg; Geoyaw *= fRad2Deg; 70 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 71 | kalmanX.setAngle(roll); 72 | kalAngleX = roll; 73 | } 74 | else{ 75 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // 卡尔曼融合 76 | } 77 | if (abs(kalAngleX) > 90) gyroYrate = -gyroYrate; // 限制的加速度计读数 78 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); //对俯仰角滤波 79 | // kalAngleZ = kalmanZ.getAngle(Geoyaw, gyroZrate, dt); //对偏航角滤波 80 | 81 | //float temperature = realVals[3] / 340.0 + 36.53; //计算温度 82 | Serial.print("accangleX:");Serial.print(roll); 83 | Serial.print(" kalAngleX:");Serial.print(kalAngleX); 84 | Serial.print(" accangleY:");Serial.print(pitch); 85 | Serial.print(" kalAngleY:");Serial.print(kalAngleY); 86 | // Serial.print(" Gryoyaw:");Serial.print(Gryoyaw); 87 | Serial.print(" Geoyaw:");Serial.print(Geoyaw); 88 | //// Serial.print(" kalAngleZ:");Serial.print(kalAngleZ); 89 | Serial.print("\r\n"); 90 | // int yrp[] = {kalAngleX,kalAngleY,Geoyaw}; 91 | // Serial.print(String(yrp[0])+','+String(yrp[1])+','+String(yrp[2])+'\n'); 92 | delay(100); 93 | } 94 | 95 | //根据俯仰角和偏航角修正地磁传感器 96 | float calculateYaw(float Pitch,float Roll,int x ,int y,int z) 97 | { 98 | x = x-offsetX; 99 | y = y-offsetY; 100 | z = z-offsetZ; 101 | float Hx,Hy; 102 | float Out; 103 | // Hx = cos(Pitch)*x+sin(Pitch)*z; 104 | // Hy = sin(Roll)*sin(Pitch)*x + cos(Roll)*y - sin(Roll)*cos(Pitch)*z; 105 | // Out = atan2(Hx,Hy); 106 | Hx = cos(Pitch)*x+sin(Roll)*sin(Pitch)*y-cos(Roll)*sin(Pitch)*z; 107 | Hy = cos(Roll)*y - sin(Roll)*z; 108 | Out = atan2(Hy,Hx); 109 | if(Out<0)Out+=2*PI; 110 | Out = Out + MagnetcDeclination; 111 | if(Out > 2*PI) Out -= 2*PI; 112 | return Out; 113 | } 114 | 115 | //从MPU6050读出加速度计三个分量、温度和三个角速度计 116 | //保存在指定的数组中 117 | void ReadAccGyr(float *pVals) { 118 | Wire.beginTransmission(MPU); 119 | Wire.write(0x3B); 120 | Wire.requestFrom(MPU, nValCnt * 2, true); 121 | Wire.endTransmission(true); 122 | for (int i = 0; i < nValCnt; ++i) { 123 | pVals[i] = Wire.read() << 8 | Wire.read(); 124 | } 125 | } 126 | 127 | //读取地磁数据 128 | void getRawData(int* x ,int* y,int* z) 129 | { 130 | Wire.beginTransmission(HMC); 131 | Wire.write(0x03); //从寄存器3开始读数据 132 | //每轴的数据都是16位的 133 | Wire.requestFrom(HMC, 6); 134 | Wire.endTransmission(); 135 | if(6<=Wire.available()){ 136 | *x = Wire.read()<<8| Wire.read(); //X msb，X轴高8位 137 | *z = Wire.read()<<8| Wire.read(); //Z msb 138 | *y = Wire.read()<<8| Wire.read(); //Y msb 139 | } 140 | } 141 | //根据xy分量计算方向角 142 | float calculateHeading(int* x ,int* y,int* z) 143 | { 144 | float headingRadians = atan2((double)((*y)-offsetY),(double)((*x)-offsetX)); 145 | // 保证数据在0-2*PI之间 146 | if(headingRadians < 0) 147 | headingRadians += 2*PI; 148 | 149 | float headingDegrees = headingRadians * fRad2Deg; 150 | headingDegrees += MagnetcDeclination; //磁偏角 151 | 152 | // 保证数据在0-360之间 153 | if(headingDegrees > 360) 154 | headingDegrees -= 360; 155 | 156 | return headingDegrees; 157 | } 158 | //校正传感器 159 | void calibrateMag() 160 | { 161 | int x,y,z; //三轴数据 162 | int xMax, xMin, yMax, yMin, zMax, zMin; 163 | //初始化 164 | getRawData(&x,&y,&z); 165 | xMax=xMin=x; 166 | yMax=yMin=y; 167 | zMax=zMin=z; 168 | offsetX = offsetY = offsetZ = 0; 169 | 170 | Serial.println("Starting Calibration......"); 171 | Serial.println("Please turn your device around in 20 seconds"); 172 | 173 | for(int i=0;i<200;i++) 174 | { 175 | getRawData(&x,&y,&z); 176 | // 计算最大值与最小值 177 | // 计算传感器绕X,Y,Z轴旋转时的磁场强度最大值和最小值 178 | if (x > xMax) 179 | xMax = x; 180 | if (x < xMin ) 181 | xMin = x; 182 | if(y > yMax ) 183 | yMax = y; 184 | if(y < yMin ) 185 | yMin = y; 186 | if(z > zMax ) 187 | zMax = z; 188 | if(z < zMin ) 189 | zMin = z; 190 | 191 | delay(100); 192 | 193 | if(i%10 == 0) 194 | { 195 | Serial.print(xMax); 196 | Serial.print(" "); 197 | Serial.println(xMin); 198 | } 199 | } 200 | //计算修正量 201 | if(abs(xMax - xMin) > CalThreshold ) 202 | offsetX = (xMax + xMin)/2; 203 | if(abs(yMax - yMin) > CalThreshold ) 204 | offsetY = (yMax + yMin)/2; 205 | if(abs(zMax - zMin) > CalThreshold ) 206 | offsetZ = (zMax +zMin)/2; 207 | 208 | Serial.print("offsetX:"); 209 | Serial.print(""); 210 | Serial.print(offsetX); 211 | Serial.print(" offsetY:"); 212 | Serial.print(""); 213 | Serial.print(offsetY); 214 | Serial.print(" offsetZ:"); 215 | Serial.print(""); 216 | Serial.println(offsetZ); 217 | } 218 | 219 | 220 | //算得Roll角。 221 | void GetRollPitch(float *pRealVals,float* roll,float* pitch) { 222 | #ifdef RESTRICT_PITCH 223 | float fNorm = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]); 224 | *pitch = atan2(-pRealVals[0],fNorm); 225 | *roll = atan2(pRealVals[1],pRealVals[2]); //atan2和atan作用相同，但atan2在除数是0时也可以计算，所以尽量使用atan2 226 | #else 227 | float fNorm = sqrt(pRealVals[2] * pRealVals[2] + pRealVals[0] * pRealVals[0]); 228 | *roll = atan2(pRealVals[1],fNorm); 229 | *pitch = atan2(-pRealVals[0],pRealVals[2]); 230 | #endif 231 | } 232 | 233 | //向MPU6050写入一个字节的数据 234 | //指定寄存器地址与一个字节的值 235 | void WriteMPUReg(int nReg, unsigned char nVal) { 236 | Wire.beginTransmission(MPU); 237 | Wire.write(nReg); 238 | Wire.write(nVal); 239 | Wire.endTransmission(true); 240 | } 241 | 242 | //从MPU6050读出一个字节的数据 243 | //指定寄存器地址，返回读出的值 244 | unsigned char ReadMPUReg(int nReg) { 245 | Wire.beginTransmission(MPU); 246 | Wire.write(nReg); 247 | Wire.requestFrom(MPU, 1, true); 248 | Wire.endTransmission(true); 249 | return Wire.read(); 250 | } 251 | --------------------------------------------------------------------------------