Hello, here is the code to process a 2D Kalman Filter by Carbon Aeronautics using BasicLinearAlgebra.h 5.0.0. I put it in my Arduino IDE latest version 2.3.2 to verify first:
/*
The contents of this code and instructions are the intellectual property of Carbon Aeronautics.
The text and figures in this code and instructions are licensed under a Creative Commons Attribution - Noncommercial - ShareAlike 4.0 International Public Licence.
This license lets you remix, adapt, and build upon your work non-commercially, as long as you credit Carbon Aeronautics
(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.
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
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
the software and hardware described in it. As Carbon Aeronautics has no control over the use, setup, assembly, modification or misuse of the hardware,
software and information described in this manual, no liability shall be assumed nor accepted for any resulting damage or injury.
By the act of copying, use, setup or assembly, the user accepts all resulting liability.
1.0 18 February 2023 - initial release
*/
#include <Wire.h>
float RateRoll, RatePitch, RateYaw;
float AngleRoll, AnglePitch;
float AccX, AccY, AccZ;
float AccZInertial;
float LoopTimer;
uint16_t dig_T1, dig_P1;
int16_t dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5;
int16_t dig_P6, dig_P7, dig_P8, dig_P9;
float AltitudeBarometer, AltitudeBarometerStartUp;
int RateCalibrationNumber;
#include <BasicLinearAlgebra.h>
using namespace BLA;
float AltitudeKalman, VelocityVerticalKalman;
BLA::Matrix<2,2> F; BLA::Matrix<2,1> G;
BLA::Matrix<2,2> P; BLA::Matrix<2,2> Q;
BLA::Matrix<2,1> S; BLA::Matrix<1,2> H;
BLA::Matrix<2,2> I; BLA::Matrix<1,1> Acc;
BLA::Matrix<2,1> K; BLA::Matrix<1,1> R;
BLA::Matrix<1,1> L; BLA::Matrix<1,1> M;
void kalman_2d(void){
Acc = {AccZInertial};
S=F*S+G*Acc;
P=F*P*~F+Q;
L=H*P*~H+R;
K=P*~H*Invert(L);
M={AltitudeBarometer};
S=S+K*(M-H*S);
AltitudeKalman=S(0,0);
VelocityVerticalKalman=S(1,0);
P=(I-K*H)*P;
}
void barometer_signals(void){
Wire.beginTransmission(0x76);
Wire.write(0xF7);
Wire.endTransmission();
Wire.requestFrom(0x76,6);
uint32_t press_msb = Wire.read();
uint32_t press_lsb = Wire.read();
uint32_t press_xlsb = Wire.read();
uint32_t temp_msb = Wire.read();
uint32_t temp_lsb = Wire.read();
uint32_t temp_xlsb = Wire.read();
unsigned long int adc_P = (press_msb << 12) | (press_lsb << 4) | (press_xlsb >>4);
unsigned long int adc_T = (temp_msb << 12) | (temp_lsb << 4) | (temp_xlsb >>4);
signed long int var1, var2;
var1 = ((((adc_T >> 3) - ((signed long int )dig_T1 <<1)))* ((signed long int )dig_T2)) >> 11;
var2 = (((((adc_T >> 4) - ((signed long int )dig_T1)) * ((adc_T>>4) - ((signed long int )dig_T1)))>> 12) * ((signed long int )dig_T3)) >> 14;
signed long int t_fine = var1 + var2;
unsigned long int p;
var1 = (((signed long int )t_fine)>>1) - (signed long int )64000;
var2 = (((var1>>2) * (var1>>2)) >> 11) * ((signed long int )dig_P6);
var2 = var2 + ((var1*((signed long int )dig_P5)) <<1);
var2 = (var2>>2)+(((signed long int )dig_P4) <<16);
var1 = (((dig_P3 * (((var1>>2)*(var1>>2)) >> 13))>>3)+((((signed long int )dig_P2) * var1)>>1))>>18;
var1 = ((((32768+var1))*((signed long int )dig_P1)) >>15);
if (var1 == 0) { p=0;}
p = (((unsigned long int )(((signed long int ) 1048576)-adc_P)-(var2>>12)))*3125;
if(p<0x80000000){ p = (p << 1) / ((unsigned long int ) var1);}
else { p = (p / (unsigned long int )var1) * 2; }
var1 = (((signed long int )dig_P9) * ((signed long int ) (((p>>3) * (p>>3))>>13)))>>12;
var2 = (((signed long int )(p>>2)) * ((signed long int )dig_P8))>>13;
p = (unsigned long int)((signed long int )p + ((var1 + var2+ dig_P7) >> 4));
double pressure=(double)p/100;
AltitudeBarometer=44330*(1-pow(pressure/1013.25, 1/5.255))*100;
}
void gyro_signals(void) {
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
int16_t AccXLSB = Wire.read() << 8 | Wire.read();
int16_t AccYLSB = Wire.read() << 8 | Wire.read();
int16_t AccZLSB = Wire.read() << 8 | Wire.read();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x8);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
int16_t GyroX=Wire.read()<<8 | Wire.read();
int16_t GyroY=Wire.read()<<8 | Wire.read();
int16_t GyroZ=Wire.read()<<8 | Wire.read();
RateRoll=(float)GyroX/65.5;
RatePitch=(float)GyroY/65.5;
RateYaw=(float)GyroZ/65.5;
AccX=(float)AccXLSB/4096;
AccY=(float)AccYLSB/4096;
AccZ=(float)AccZLSB/4096;
AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*AccZ))*1/(3.142/180);
AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*AccZ))*1/(3.142/180);
}
void setup() {
Serial.begin(57600);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
Wire.setClock(400000);
Wire.begin();
delay(250);
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
Wire.beginTransmission(0x76);
Wire.write(0xF4);
Wire.write(0x57);
Wire.endTransmission();
Wire.beginTransmission(0x76);
Wire.write(0xF5);
Wire.write(0x14);
Wire.endTransmission();
uint8_t data[24], i=0;
Wire.beginTransmission(0x76);
Wire.write(0x88);
Wire.endTransmission();
Wire.requestFrom(0x76,24);
while(Wire.available()){
data[i] = Wire.read();
i++;
}
dig_T1 = (data[1] << 8) | data[0];
dig_T2 = (data[3] << 8) | data[2];
dig_T3 = (data[5] << 8) | data[4];
dig_P1 = (data[7] << 8) | data[6];
dig_P2 = (data[9] << 8) | data[8];
dig_P3 = (data[11]<< 8) | data[10];
dig_P4 = (data[13]<< 8) | data[12];
dig_P5 = (data[15]<< 8) | data[14];
dig_P6 = (data[17]<< 8) | data[16];
dig_P7 = (data[19]<< 8) | data[18];
dig_P8 = (data[21]<< 8) | data[20];
dig_P9 = (data[23]<< 8) | data[22]; delay(250);
for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) {
barometer_signals();
AltitudeBarometerStartUp+=AltitudeBarometer;
delay(1);
}
AltitudeBarometerStartUp/=2000;
F = {1, 0.004,
0, 1};
G = {0.5*0.004*0.004,
0.004};
H = {1, 0};
I = {1, 0,
0, 1};
Q = G * ~G*10*10;
R = {30*30};
P = {0, 0,
0, 0};
S = {0,
0};
LoopTimer=micros();
}
void loop() {
gyro_signals();
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;
AccZInertial=(AccZInertial-1)*9.81*100;
barometer_signals();
AltitudeBarometer-=AltitudeBarometerStartUp;
kalman_2d();
Serial.print("Altitude [cm]: ");
Serial.print(AltitudeKalman);
Serial.print(" Vertical velocity [cm/s]: ");
Serial.println(VelocityVerticalKalman);
while (micros() - LoopTimer < 4000);
LoopTimer=micros();
}
Verifying gives a lot of errors but in Carbon Aeronautics's video 19, he had no problem at all. I am using ESP32 Dev Kit.
In file included from c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/BasicLinearAlgebra.h:10,
from C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:25:
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/ElementStorage.h:136:43: error: expected unqualified-id before 'const'
136 | HorizontalConcat<LeftType, RightType>(const LeftType &l, const RightType &r) : left(l), right(r) {}
| ^~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/ElementStorage.h:136:43: error: expected ')' before 'const'
136 | HorizontalConcat<LeftType, RightType>(const LeftType &l, const RightType &r) : left(l), right(r) {}
| ~^~~~~
| )
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/ElementStorage.h:151:41: error: expected unqualified-id before 'const'
151 | VerticalConcat<TopType, BottomType>(const TopType &t, const BottomType &b) : top(t), bottom(b) {}
| ^~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/ElementStorage.h:151:41: error: expected ')' before 'const'
151 | VerticalConcat<TopType, BottomType>(const TopType &t, const BottomType &b) : top(t), bottom(b) {}
| ~^~~~~
| )
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino: In function 'void kalman_2d()':
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:39:9: error: no match for 'operator*' (operand types are 'BLA::Matrix<2, 1>' and 'bool')
39 | K=P*~H*Invert(L);
| ~~~~^~~~~~~~~~
| | |
| | bool
| BLA::Matrix<2, 1>
In file included from c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/BasicLinearAlgebra.h:181:
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:10:35: note: candidate: 'template<class MatAType, class MatBType, int MatARows, int MatACols, int MatBCols, class DType> BLA::Matrix<MatARows, MatBCols, DType> BLA::operator*(const MatrixBase<MatAType, MatARows, MatACols, DType>&, const MatrixBase<MatBType, MatACols, MatBCols, DType>&)'
10 | Matrix<MatARows, MatBCols, DType> operator*(const MatrixBase<MatAType, MatARows, MatACols, DType> &matA,
| ^~~~~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:10:35: note: template argument deduction/substitution failed:
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:39:18: note: mismatched types 'const BLA::MatrixBase<MatBType, MatACols, MatBCols, DType>' and 'bool'
39 | K=P*~H*Invert(L);
| ^
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:168:27: note: candidate: 'template<class MatType, int Rows, int Cols, class DType> BLA::Matrix<Rows, Cols, DType> BLA::operator*(const MatrixBase<MatType, Rows, Cols, DType>&, DType)'
168 | Matrix<Rows, Cols, DType> operator*(const MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
| ^~~~~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:168:27: note: template argument deduction/substitution failed:
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:39:18: note: deduced conflicting types for parameter 'DType' ('float' and 'bool')
39 | K=P*~H*Invert(L);
| ^
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:176:27: note: candidate: 'template<class MatType, int Rows, int Cols, class DType> BLA::Matrix<Rows, Cols, DType> BLA::operator*(DType, const MatrixBase<MatType, Rows, Cols, DType>&)'
176 | Matrix<Rows, Cols, DType> operator*(const DType k, const MatrixBase<MatType, Rows, Cols, DType> &mat)
| ^~~~~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:176:27: note: template argument deduction/substitution failed:
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:39:18: note: mismatched types 'const BLA::MatrixBase<MatType, Rows, Cols, DType>' and 'bool'
39 | K=P*~H*Invert(L);
| ^
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino: In function 'void setup()':
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:169:13: error: no match for 'operator*' (operand types are 'BLA::Matrix<2, 2>' and 'int')
169 | Q = G * ~G*10*10;
| ~~~~~~^~~
| | |
| | int
| BLA::Matrix<2, 2>
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:10:35: note: candidate: 'template<class MatAType, class MatBType, int MatARows, int MatACols, int MatBCols, class DType> BLA::Matrix<MatARows, MatBCols, DType> BLA::operator*(const MatrixBase<MatAType, MatARows, MatACols, DType>&, const MatrixBase<MatBType, MatACols, MatBCols, DType>&)'
10 | Matrix<MatARows, MatBCols, DType> operator*(const MatrixBase<MatAType, MatARows, MatACols, DType> &matA,
| ^~~~~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:10:35: note: template argument deduction/substitution failed:
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:169:14: note: mismatched types 'const BLA::MatrixBase<MatBType, MatACols, MatBCols, DType>' and 'int'
169 | Q = G * ~G*10*10;
| ^~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:168:27: note: candidate: 'template<class MatType, int Rows, int Cols, class DType> BLA::Matrix<Rows, Cols, DType> BLA::operator*(const MatrixBase<MatType, Rows, Cols, DType>&, DType)'
168 | Matrix<Rows, Cols, DType> operator*(const MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
| ^~~~~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:168:27: note: template argument deduction/substitution failed:
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:169:14: note: deduced conflicting types for parameter 'DType' ('float' and 'int')
169 | Q = G * ~G*10*10;
| ^~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:176:27: note: candidate: 'template<class MatType, int Rows, int Cols, class DType> BLA::Matrix<Rows, Cols, DType> BLA::operator*(DType, const MatrixBase<MatType, Rows, Cols, DType>&)'
176 | Matrix<Rows, Cols, DType> operator*(const DType k, const MatrixBase<MatType, Rows, Cols, DType> &mat)
| ^~~~~~~~
c:\Users\Admin\Documents\Arduino\libraries\BasicLinearAlgebra/impl/BasicLinearAlgebra.h:176:27: note: template argument deduction/substitution failed:
C:\Users\Admin\AppData\Local\Temp\.arduinoIDE-unsaved2024529-3900-igtl2t.od57r\sketch_jun29a\sketch_jun29a.ino:169:14: note: mismatched types 'const BLA::MatrixBase<MatType, Rows, Cols, DType>' and 'int'
169 | Q = G * ~G*10*10;
| ^~
exit status 1
Compilation error: no match for 'operator*' (operand types are 'BLA::Matrix<2, 1>' and 'bool')
There are so many errors I don't know what to do. For errors that say can't multiply the matrix with a constant or Boolean Inverted Matrix, I can fix them by making new functions but I thought the library should have included it, the other errors lie in the ElementStorage.h, which I don't understand. Thank you. I don't understand why Im stucked and why does this library work for other people?