Kalman Filter Code for Yellow Plane 2 Special thanks to Kristian Lauszus, TKJ Electronics

This is the code in the main loop UpdateServos()

<code>

unsigned long msDelta = LastMicros - micros();

LastMicros = micros();

//Measure time since last cycle

double dt = (double)msDelta / 1000000.0;

// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

double X_Angle = (double)AnIn[0];

double X_Rate = (double)AnIn[4];

double Kalman_X = kalman[0].getAngle(X_Angle, X_Rate, dt);

double Y_Angle = (double)AnIn[1];

double Y_Rate = (double)AnIn[5];

double Kalman_Y = kalman[1].getAngle(Y_Angle, Y_Rate, dt);

double Z_Angle = (double)AnIn[2];

double Z_Rate = (double)AnIn[6];

double Kalman_Z = kalman[2].getAngle(Z_Angle, Z_Rate, dt);

</code>

kalman.h can be downloaded here

<code>

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

This software may be distributed and modified under the terms of the GNU

General Public License version 2 (GPL2) as published by the Free Software

Foundation and appearing in the file GPL2.TXT included in the packaging of

this file. Please note that GPL2 Section 2[b] requires that all works based

on this software must also be made publicly available under the terms of

the GPL2 ("Copyleft").

Contact information

-------------------

Kristian Lauszus, TKJ Electronics

Web : http://www.tkjelectronics.com

e-mail : kristianl@tkjelectronics.com

*/

#ifndef _Kalman_h

#define _Kalman_h

class Kalman {

public:

Kalman() {

/* We will set the varibles like so, these can also be tuned by the user */

Q_angle = 0.001;

Q_bias = 0.003;

R_measure = 0.03;

bias = 0; // Reset bias

P[0][0] = 0; // Since we assume tha 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

P[0][1] = 0;

P[1][0] = 0;

P[1][1] = 0;

};

// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

double getAngle(double newAngle, double newRate, double dt) {

// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145

// Modified by Kristian Lauszus

// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

// Discrete Kalman filter time update equations - Time Update ("Predict")

// Update xhat - Project the state ahead

/* Step 1 */

rate = newRate - bias;

angle += dt * rate;

// Update estimation error covariance - Project the error covariance ahead

/* Step 2 */

P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);

P[0][1] -= dt * P[1][1];

P[1][0] -= dt * P[1][1];

P[1][1] += Q_bias * dt;

// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")

// Calculate Kalman gain - Compute the Kalman gain

/* Step 4 */

S = P[0][0] + R_measure;

/* Step 5 */

K[0] = P[0][0] / S;

K[1] = P[1][0] / S;

// Calculate angle and bias - Update estimate with measurement zk (newAngle)

/* Step 3 */

y = newAngle - angle;

/* Step 6 */

angle += K[0] * y;

bias += K[1] * y;

// Calculate estimation error covariance - Update the error covariance

/* Step 7 */

P[0][0] -= K[0] * P[0][0];

P[0][1] -= K[0] * P[0][1];

P[1][0] -= K[1] * P[0][0];

P[1][1] -= K[1] * P[0][1];

return angle;

};

void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle

double getRate() { return rate; }; // Return the unbiased rate

/* These are used to tune the Kalman filter */

void setQangle(double newQ_angle) { Q_angle = newQ_angle; };

void setQbias(double newQ_bias) { Q_bias = newQ_bias; };

void setRmeasure(double newR_measure) { R_measure = newR_measure; };

private:

/* variables */

double Q_angle; // Process noise variance for the accelerometer

double Q_bias; // Process noise variance for the gyro bias

double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise

double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix

double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix

double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

double P[2][2]; // Error covariance matrix - This is a 2x2 matrix

double K[2]; // Kalman gain - This is a 2x1 matrix

double y; // Angle difference - 1x1 matrix

double S; // Estimate error - 1x1 matrix

};

#endif

</code>

The Arduino code originally written by Kristian Lauszus, TKJ Electronics can be

downloaded here

I have a test harness written in C# there is a C# version of the Kalman Filter class which could very easyly be addapted for use with Fex Panda or Netduino Tiny CLR projects

The C# code for the test harness shown below can be downloaded here

The above could be modified to accept input from a IMU or Accelerometers and Gyros

## Comments

I like the simplicity myself, given the time is available I will be hooking it up to the sensors and then we can see if its cosher, should be interesting

This kalman implementation looks to be very simplified. Perhaps to much so for a plane? Anyways, it is going to be interesting to see how it compares to DCM.