├── THUMBNAIL_YOUTUBE.png ├── README.md └── ArduinoCode /THUMBNAIL_YOUTUBE.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CarbonAeronautics/Part-XIX-2D-Kalman-filter/HEAD/THUMBNAIL_YOUTUBE.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Part XIX: two-dimensional Kalman filter 2 | 3 | In this part, you will learn how you can measure the vertical velocity and altitude of your quadcopter by combining the measurements of the MPU6050 accelerometer and BMP280 altitude sensors through a two-dimensional Kalman filter. All explanations can be found in the Youtube video below, and the full code is given in this repository. 4 | 5 | [![alt text](https://github.com/CarbonAeronautics/Part-XIX-2D-Kalman-filter/blob/77684722fc629fc8f234d2d400d4b0e6c171be5d/THUMBNAIL_YOUTUBE.png?raw=true)](https://www.youtube.com/watch?v=GZevJyabMdI) 6 | -------------------------------------------------------------------------------- /ArduinoCode: -------------------------------------------------------------------------------- 1 | /* 2 | The contents of this code and instructions are the intellectual property of Carbon Aeronautics. 3 | The text and figures in this code and instructions are licensed under a Creative Commons Attribution - Noncommercial - ShareAlike 4.0 International Public Licence. 4 | This license lets you remix, adapt, and build upon your work non-commercially, as long as you credit Carbon Aeronautics 5 | (but not in any way that suggests that we endorse you or your use of the work) and license your new creations under the identical terms. 6 | This code and instruction is provided "As Is” without any further warranty. Neither Carbon Aeronautics or the author has any liability to any person or entity 7 | with respect to any loss or damage caused or declared to be caused directly or indirectly by the instructions contained in this code or by 8 | the software and hardware described in it. As Carbon Aeronautics has no control over the use, setup, assembly, modification or misuse of the hardware, 9 | software and information described in this manual, no liability shall be assumed nor accepted for any resulting damage or injury. 10 | By the act of copying, use, setup or assembly, the user accepts all resulting liability. 11 | 12 | 1.0 18 February 2023 - initial release 13 | */ 14 | #include 15 | float RateRoll, RatePitch, RateYaw; 16 | float AngleRoll, AnglePitch; 17 | float AccX, AccY, AccZ; 18 | float AccZInertial; 19 | float LoopTimer; 20 | uint16_t dig_T1, dig_P1; 21 | int16_t dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5; 22 | int16_t dig_P6, dig_P7, dig_P8, dig_P9; 23 | float AltitudeBarometer, AltitudeBarometerStartUp; 24 | int RateCalibrationNumber; 25 | #include 26 | using namespace BLA; 27 | float AltitudeKalman, VelocityVerticalKalman; 28 | BLA::Matrix<2,2> F; BLA::Matrix<2,1> G; 29 | BLA::Matrix<2,2> P; BLA::Matrix<2,2> Q; 30 | BLA::Matrix<2,1> S; BLA::Matrix<1,2> H; 31 | BLA::Matrix<2,2> I; BLA::Matrix<1,1> Acc; 32 | BLA::Matrix<2,1> K; BLA::Matrix<1,1> R; 33 | BLA::Matrix<1,1> L; BLA::Matrix<1,1> M; 34 | void kalman_2d(void){ 35 | Acc = {AccZInertial}; 36 | S=F*S+G*Acc; 37 | P=F*P*~F+Q; 38 | L=H*P*~H+R; 39 | K=P*~H*Invert(L); 40 | M={AltitudeBarometer}; 41 | S=S+K*(M-H*S); 42 | AltitudeKalman=S(0,0); 43 | VelocityVerticalKalman=S(1,0); 44 | P=(I-K*H)*P; 45 | } 46 | void barometer_signals(void){ 47 | Wire.beginTransmission(0x76); 48 | Wire.write(0xF7); 49 | Wire.endTransmission(); 50 | Wire.requestFrom(0x76,6); 51 | uint32_t press_msb = Wire.read(); 52 | uint32_t press_lsb = Wire.read(); 53 | uint32_t press_xlsb = Wire.read(); 54 | uint32_t temp_msb = Wire.read(); 55 | uint32_t temp_lsb = Wire.read(); 56 | uint32_t temp_xlsb = Wire.read(); 57 | unsigned long int adc_P = (press_msb << 12) | (press_lsb << 4) | (press_xlsb >>4); 58 | unsigned long int adc_T = (temp_msb << 12) | (temp_lsb << 4) | (temp_xlsb >>4); 59 | signed long int var1, var2; 60 | var1 = ((((adc_T >> 3) - ((signed long int )dig_T1 <<1)))* ((signed long int )dig_T2)) >> 11; 61 | var2 = (((((adc_T >> 4) - ((signed long int )dig_T1)) * ((adc_T>>4) - ((signed long int )dig_T1)))>> 12) * ((signed long int )dig_T3)) >> 14; 62 | signed long int t_fine = var1 + var2; 63 | unsigned long int p; 64 | var1 = (((signed long int )t_fine)>>1) - (signed long int )64000; 65 | var2 = (((var1>>2) * (var1>>2)) >> 11) * ((signed long int )dig_P6); 66 | var2 = var2 + ((var1*((signed long int )dig_P5)) <<1); 67 | var2 = (var2>>2)+(((signed long int )dig_P4) <<16); 68 | var1 = (((dig_P3 * (((var1>>2)*(var1>>2)) >> 13))>>3)+((((signed long int )dig_P2) * var1)>>1))>>18; 69 | var1 = ((((32768+var1))*((signed long int )dig_P1)) >>15); 70 | if (var1 == 0) { p=0;} 71 | p = (((unsigned long int )(((signed long int ) 1048576)-adc_P)-(var2>>12)))*3125; 72 | if(p<0x80000000){ p = (p << 1) / ((unsigned long int ) var1);} 73 | else { p = (p / (unsigned long int )var1) * 2; } 74 | var1 = (((signed long int )dig_P9) * ((signed long int ) (((p>>3) * (p>>3))>>13)))>>12; 75 | var2 = (((signed long int )(p>>2)) * ((signed long int )dig_P8))>>13; 76 | p = (unsigned long int)((signed long int )p + ((var1 + var2+ dig_P7) >> 4)); 77 | double pressure=(double)p/100; 78 | AltitudeBarometer=44330*(1-pow(pressure/1013.25, 1/5.255))*100; 79 | } 80 | void gyro_signals(void) { 81 | Wire.beginTransmission(0x68); 82 | Wire.write(0x1A); 83 | Wire.write(0x05); 84 | Wire.endTransmission(); 85 | Wire.beginTransmission(0x68); 86 | Wire.write(0x1C); 87 | Wire.write(0x10); 88 | Wire.endTransmission(); 89 | Wire.beginTransmission(0x68); 90 | Wire.write(0x3B); 91 | Wire.endTransmission(); 92 | Wire.requestFrom(0x68,6); 93 | int16_t AccXLSB = Wire.read() << 8 | Wire.read(); 94 | int16_t AccYLSB = Wire.read() << 8 | Wire.read(); 95 | int16_t AccZLSB = Wire.read() << 8 | Wire.read(); 96 | Wire.beginTransmission(0x68); 97 | Wire.write(0x1B); 98 | Wire.write(0x8); 99 | Wire.endTransmission(); 100 | Wire.beginTransmission(0x68); 101 | Wire.write(0x43); 102 | Wire.endTransmission(); 103 | Wire.requestFrom(0x68,6); 104 | int16_t GyroX=Wire.read()<<8 | Wire.read(); 105 | int16_t GyroY=Wire.read()<<8 | Wire.read(); 106 | int16_t GyroZ=Wire.read()<<8 | Wire.read(); 107 | RateRoll=(float)GyroX/65.5; 108 | RatePitch=(float)GyroY/65.5; 109 | RateYaw=(float)GyroZ/65.5; 110 | AccX=(float)AccXLSB/4096; 111 | AccY=(float)AccYLSB/4096; 112 | AccZ=(float)AccZLSB/4096; 113 | AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*AccZ))*1/(3.142/180); 114 | AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*AccZ))*1/(3.142/180); 115 | } 116 | void setup() { 117 | Serial.begin(57600); 118 | pinMode(13, OUTPUT); 119 | digitalWrite(13, HIGH); 120 | Wire.setClock(400000); 121 | Wire.begin(); 122 | delay(250); 123 | Wire.beginTransmission(0x68); 124 | Wire.write(0x6B); 125 | Wire.write(0x00); 126 | Wire.endTransmission(); 127 | Wire.beginTransmission(0x76); 128 | Wire.write(0xF4); 129 | Wire.write(0x57); 130 | Wire.endTransmission(); 131 | Wire.beginTransmission(0x76); 132 | Wire.write(0xF5); 133 | Wire.write(0x14); 134 | Wire.endTransmission(); 135 | uint8_t data[24], i=0; 136 | Wire.beginTransmission(0x76); 137 | Wire.write(0x88); 138 | Wire.endTransmission(); 139 | Wire.requestFrom(0x76,24); 140 | while(Wire.available()){ 141 | data[i] = Wire.read(); 142 | i++; 143 | } 144 | dig_T1 = (data[1] << 8) | data[0]; 145 | dig_T2 = (data[3] << 8) | data[2]; 146 | dig_T3 = (data[5] << 8) | data[4]; 147 | dig_P1 = (data[7] << 8) | data[6]; 148 | dig_P2 = (data[9] << 8) | data[8]; 149 | dig_P3 = (data[11]<< 8) | data[10]; 150 | dig_P4 = (data[13]<< 8) | data[12]; 151 | dig_P5 = (data[15]<< 8) | data[14]; 152 | dig_P6 = (data[17]<< 8) | data[16]; 153 | dig_P7 = (data[19]<< 8) | data[18]; 154 | dig_P8 = (data[21]<< 8) | data[20]; 155 | dig_P9 = (data[23]<< 8) | data[22]; delay(250); 156 | for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) { 157 | barometer_signals(); 158 | AltitudeBarometerStartUp+=AltitudeBarometer; 159 | delay(1); 160 | } 161 | AltitudeBarometerStartUp/=2000; 162 | F = {1, 0.004, 163 | 0, 1}; 164 | G = {0.5*0.004*0.004, 165 | 0.004}; 166 | H = {1, 0}; 167 | I = {1, 0, 168 | 0, 1}; 169 | Q = G * ~G*10*10; 170 | R = {30*30}; 171 | P = {0, 0, 172 | 0, 0}; 173 | S = {0, 174 | 0}; 175 | LoopTimer=micros(); 176 | } 177 | void loop() { 178 | gyro_signals(); 179 | AccZInertial=-sin(AnglePitch*(3.142/180))*AccX+cos(AnglePitch*(3.142/180))*sin(AngleRoll*(3.142/180))* AccY+cos(AnglePitch*(3.142/180))*cos(AngleRoll*(3.142/180))*AccZ; 180 | AccZInertial=(AccZInertial-1)*9.81*100; 181 | barometer_signals(); 182 | AltitudeBarometer-=AltitudeBarometerStartUp; 183 | kalman_2d(); 184 | Serial.print("Altitude [cm]: "); 185 | Serial.print(AltitudeKalman); 186 | Serial.print(" Vertical velocity [cm/s]: "); 187 | Serial.println(VelocityVerticalKalman); 188 | while (micros() - LoopTimer < 4000); 189 | LoopTimer=micros(); 190 | } 191 | --------------------------------------------------------------------------------