Kalman filter for TinyCLR

Always wanted one of these for my Yellow Plane

A peice of C# converted from Arduino C original author Kristian Lauszus, TKJ Electronics here is my wee test harness for same.

Here is the test harness the whole project is attached in a zip along with the two classes if that all you need 

See Kalman filter

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.This software may be distributed and modified under the terms of the GNUGeneral Public License version 2 (GPL2) as published by the Free SoftwareFoundation and appearing in the file GPL2.TXT included in the packaging ofthis file. Please note that GPL2 Section 2[b] requires that all works basedon this software must also be made publicly available under the terms ofthe GPL2 ("Copyleft").Contact information-------------------Kristian Lauszus, TKJ ElectronicsWeb : http://www.tkjelectronics.come-mail : kristianl@tkjelectronics.com*/using System;using System.Collections.Generic;using System.Linq;using System.Text;namespace KalmanTest{public class Kalman{/* variables */double Q_angle; // Process noise variance for the accelerometerdouble Q_bias; // Process noise variance for the gyro biasdouble R_measure; // Measurement noise variance - this is actually the variance of the measurement noisedouble angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrixdouble bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrixdouble rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the ratedouble[,] P = new double[2,2]; // Error covariance matrix - This is a 2x2 matrixdouble[] K = new double[2]; // Kalman gain - This is a 2x1 matrixdouble y; // Angle difference - 1x1 matrixdouble S; // Estimate error - 1x1 matrixpublic Kalman() {/* We will set the varibles like so, these can also be tuned by the user */Reset();// Reset bias// 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_technicalSetDefaults();}// The angle should be in degrees and the rate should be in degrees per second and the delta time in secondspublic 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;}public void setAngle(double newAngle) { angle = newAngle; } // Used to set angle, this should be set as the starting anglepublic double getRate() { return rate; } // Return the unbiased rate/* These are used to tune the Kalman filter */public void setQangle(double newQ_angle) { Q_angle = newQ_angle; }public double getQangle() { return Q_angle; }public void setQbias(double newQ_bias) { Q_bias = newQ_bias; }public double getQbias() { return Q_bias; }public void setRmeasure(double newR_measure) { R_measure = newR_measure; }public double getRmeasure() { return R_measure; }public void Reset(){bias = 0; // Reset biasP[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_technicalP[0, 1] = 0;P[1, 0] = 0;P[1, 1] = 0;}public void SetDefaults(){/* 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;}}//End of class}