Blogs

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