Hello all, thanks to Carbon Aeronautics, I converted his code (episode 19, using 2d Kalman filter to get altitude and vertical velocity) into a code that uses 2d kalman filter, but using MS5611 in GY86, not BMP280. The results however is very bad, I couldn't reach those 2 values around 0, the integral summing is so bad the plot is a straight down line, and when I lift the GY86 up, no more results show. I am using Arduino Uno and GY86. The third problem is that after plugging in the Arduino, it takes a very long time, about 20 30 seconds to start showing results. I tried changing the standard deviation but there is no luck:
#include <Wire.h>
#include <MS5611.h>
float RateRoll, RatePitch, RateYaw;
float AngleRoll, AnglePitch;
float AccX, AccY, AccZ;
float AccZInertial;
float LoopTimer;
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;
BLA::Matrix<1,1> inv_L = Inverse(L);
MS5611 ms5611;
void kalman_2d(void){
Acc = {AccZInertial};
S=F*S+G*Acc;
P=F*P*~F+Q;
L=H*P*~H+R;
K=P*~H*inv_L;
M={AltitudeBarometer};
S=S+K*(M-H*S);
AltitudeKalman=S(0,0);
VelocityVerticalKalman=S(1,0);
P=(I-K*H)*P;
}
double referencePressure;
void checkSettings()
{
Serial.print("Oversampling: ");
Serial.println(ms5611.getOversampling());
}
void baro(void){
long realPressure = ms5611.readPressure();
float AltitudeBarometer = ms5611.getAltitude(realPressure, referencePressure) * 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);
while (!ms5611.begin())
{
Serial.println("Could not find a valid MS5611 sensor, check wiring!");
delay(500);
}
referencePressure = ms5611.readPressure();
Wire.setClock(400000);
Wire.begin();
delay(250);
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
// Check settings
checkSettings();
delay(500);
for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) {
baro();
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.0f*10.0f;
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;
baro();
AltitudeBarometer-=AltitudeBarometerStartUp;
kalman_2d();
Serial.print("Altitude [m]:");
Serial.print(AltitudeKalman/100);
Serial.print(" Vertical velocity [m/s]:");
Serial.println(VelocityVerticalKalman/100);
while (micros() - LoopTimer < 4000);
LoopTimer=micros();
}
Here is the result:
Thank you