The problem was caused by the initialization code: your unit was failing the “stillness” test during the calibration and therefore reading in a previously-stored value for some parameters. Unfortunately as it was new hardware no value had ever been stored so what was being read back was trash and causing the calculations to fail.

The workaround was to look in the file ApplicationRoutines and change the line
if (vAccelVariance.magnitude() > 5.0e-4) {

to
if (FALSE) {

for one run.

You should probably investigate what a sensible value for the condition test there is by printing and examining values of vAccelVariance.magnitude() for different conditions; you want the “if” condition to succeed if the unit is sitting still on a table, but not if (for example) you’re holding it in your hand.

ReadGyroAccel();
Serial.print(10 * RADTODEG * roll,0);
Serial.print(“,”);
Serial.print(10 * RADTODEG * pitch,0);
Serial.println();
if (compassCounter > 10) { //20Hz
compassCounter = 0;
ReadCompass();
CorrectHeading();
}
#if MONGOOSE == 1
if (baroCounter > 100) { //2Hz
baroCounter = 0;
ReadTemperature();
ReadAltitude();
PrintBaro();
}
#endif
if (powerOnCounter 200) { //1 Hz
powerCounter = 0;
ReadBattery();
PrintPower();
}

You (or we) will need to do some debugging to see what’s going wrong. What’s your programming skill level?

Init Baro…
Calibrating device parameters…
Starting Calibration loop…… finished.
Variance of acceleration * 10000 28.16
Detected motion during calibration. Using stored values.
Beginning operation.
$RPYL,nan,nan,nan,-46,0,1001,0,
$RPYL,nan,nan,nan,-78,nan,1002,0,
$RPYL,nan,nan,nan,-103,nan,1010,0,
$RPYL,nan,nan,nan,-128,nan,1011,0,
$RPYL,nan,nan,nan,-152,nan,1016,0,
$RPYL,nan,nan,nan,-166,nan,1019,0,

The roll,pitch, and yaw readings aren’t making it…. any ideas?

Don

Thanks for the kind words. Feel free to ask any questions if something’s not clear and I’ll do my best to answer.
To get an airspeed readout is going to take some more work on your part; you’re going to need to connect a second pressure transducer to your pitot line (which of course I can’t do, with a certified aircraft). But it shouldn’t be impossible.

By the way have you seen the new Dynon D1 Pocket Panel? Kind of like the Mini G, with a display built in.

You can see there that x_hat is the state estimator, z is the latest (noisy) measurement, and y_tilde is the residual – the difference between the two, at the time of the update.

In a more general Kalman filter the elements of the state vector and the observation vector have a general linear relationship represented by the matrix H – as the Wikipedia page puts it, H “is the observation model which maps the true state space into the observed space”. In this very simple Kalman filter the state space vector has two entries – the angle to north, and the angular velocity – both of which we measure directly, so H is actually an identity matrix and drops out of the mathematics.

A further simplification is that the measurement of the magnetic angle is stored in the form of a displacement of DCM north to measured magnetic north, so it’s already exactly the residual – there is nothing to subtract – so that gives you the u- component of the y_tilde 2-vector.

The v- component of the y_tilde residual is the difference between the observed angular velocity about the earth-z axis and the estimator of that same variable.

The nice thing about implementing a Kalman Filter is that you get the ducks lined up in a row by thinking carefully about what the elements of the state vector should be, and how to calculate the residual from your measurements. After that you simply crank the handle of the equations and make use of the fact that the theory is already accounted for.

I hope that makes sense. Please remember also that I know very little about Kalman Filters and this is my only attempt to implement one. There may be errors in what I have done!

//Calculate residual:
//y_tilde = zed – x_hat
//can do this componentwise size H is identity
//angle resid. observed directly
y_tilde.u = atan2(-vMag.y, vMag.x);

//this depends only on roll and pitch angles, not heading:
y_tilde.v = vGyro.dot(R.Z) – x_hat.v;

The line number is probably off because I have converted the file to C++ adding header file includes, etc.

Which line are you looking at?

I am working on a robotics project using gyro and accelerometer. A GPS or magnetometer cannot be used for the project. It has been to many years since I took math in college so it is hard going to understand the theory papers. So I’ve been looking for code.

My project runs on a PC in standard C++. I grabbed your code to work with but am hung up on the meaning of the variable ‘zed’ that is used to in a calculation for “y_tilde”. Can you tell me what it is supposed to be?

The project information in on my website if you are interested.

Rud