Diff: Kalman/Kalman.h

Revision:

0:12fb9cbcabcc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.h	Wed Jul 24 20:03:52 2019 +0000
@@ -0,0 +1,60 @@
+/* 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();
+
+    // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
+    float getAngle(float newAngle, float newRate, float dt);
+
+    void setAngle(float angle); // Used to set angle, this should be set as the starting angle
+    float getRate(); // Return the unbiased rate
+
+    /* These are used to tune the Kalman filter */
+    void setQangle(float Q_angle);
+    /**
+     * setQbias(float Q_bias)
+     * Default value (0.003f) is in Kalman.cpp. 
+     * Raise this to follow input more closely,
+     * lower this to smooth result of kalman filter.
+     */
+    void setQbias(float Q_bias);
+    void setRmeasure(float R_measure);
+
+    float getQangle();
+    float getQbias();
+    float getRmeasure();
+
+private:
+    /* Kalman filter variables */
+    float Q_angle; // Process noise variance for the accelerometer
+    float Q_bias; // Process noise variance for the gyro bias
+    float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
+
+    float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
+    float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
+    float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
+
+    float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
+};
+
+#endif
+