diff --git a/arduino/Betas/RGB_V1.1.1/Kalman.cpp b/arduino/Betas/RGB_V1.1.1/Kalman.cpp deleted file mode 100644 index a7c70c1..0000000 --- a/arduino/Betas/RGB_V1.1.1/Kalman.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* 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 - */ - -#include "Kalman.h" - -Kalman::Kalman() { - /* We will set the variables like so, these can also be tuned by the user */ - Q_angle = 0.001f; - Q_bias = 0.003f; - R_measure = 0.03f; - - angle = 0.0f; // Reset the angle - bias = 0.0f; // Reset bias - - P[0][0] = 0.0f; // Since we assume that 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.0f; - P[1][0] = 0.0f; - P[1][1] = 0.0f; -}; - -// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds -float Kalman::getAngle(float newAngle, float newRate, float 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 */ - float S = P[0][0] + R_measure; // Estimate error - /* Step 5 */ - float K[2]; // Kalman gain - This is a 2x1 vector - K[0] = P[0][0] / S; - K[1] = P[1][0] / S; - - // Calculate angle and bias - Update estimate with measurement zk (newAngle) - /* Step 3 */ - float y = newAngle - angle; // Angle difference - /* Step 6 */ - angle += K[0] * y; - bias += K[1] * y; - - // Calculate estimation error covariance - Update the error covariance - /* Step 7 */ - float P00_temp = P[0][0]; - float P01_temp = P[0][1]; - - P[0][0] -= K[0] * P00_temp; - P[0][1] -= K[0] * P01_temp; - P[1][0] -= K[1] * P00_temp; - P[1][1] -= K[1] * P01_temp; - - return angle; -}; - -void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle -float Kalman::getRate() { return this->rate; }; // Return the unbiased rate - -/* These are used to tune the Kalman filter */ -void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; }; -void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; }; -void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; }; - -float Kalman::getQangle() { return this->Q_angle; }; -float Kalman::getQbias() { return this->Q_bias; }; -float Kalman::getRmeasure() { return this->R_measure; };