Einstein Filho / Mbed OS MANGUEBAJA2019_FRONT2

Mangue Baja team's code to frontal ECU

Kalman/Kalman.h@0:12fb9cbcabcc, 2019-07-24 (annotated)

Committer:
einsteingustavo
Date:
Wed Jul 24 20:03:52 2019 +0000
Revision:
0:12fb9cbcabcc
Mangue Baja team's code to frontal ECU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
einsteingustavo 0:12fb9cbcabcc 1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
einsteingustavo 0:12fb9cbcabcc 2
einsteingustavo 0:12fb9cbcabcc 3 This software may be distributed and modified under the terms of the GNU
einsteingustavo 0:12fb9cbcabcc 4 General Public License version 2 (GPL2) as published by the Free Software
einsteingustavo 0:12fb9cbcabcc 5 Foundation and appearing in the file GPL2.TXT included in the packaging of
einsteingustavo 0:12fb9cbcabcc 6 this file. Please note that GPL2 Section 2[b] requires that all works based
einsteingustavo 0:12fb9cbcabcc 7 on this software must also be made publicly available under the terms of
einsteingustavo 0:12fb9cbcabcc 8 the GPL2 ("Copyleft").
einsteingustavo 0:12fb9cbcabcc 9
einsteingustavo 0:12fb9cbcabcc 10 Contact information
einsteingustavo 0:12fb9cbcabcc 11 -------------------
einsteingustavo 0:12fb9cbcabcc 12
einsteingustavo 0:12fb9cbcabcc 13 Kristian Lauszus, TKJ Electronics
einsteingustavo 0:12fb9cbcabcc 14 Web : http://www.tkjelectronics.com
einsteingustavo 0:12fb9cbcabcc 15 e-mail : kristianl@tkjelectronics.com
einsteingustavo 0:12fb9cbcabcc 16 */
einsteingustavo 0:12fb9cbcabcc 17
einsteingustavo 0:12fb9cbcabcc 18 #ifndef KALMAN_H
einsteingustavo 0:12fb9cbcabcc 19 #define KALMAN_H
einsteingustavo 0:12fb9cbcabcc 20
einsteingustavo 0:12fb9cbcabcc 21 class Kalman {
einsteingustavo 0:12fb9cbcabcc 22 public:
einsteingustavo 0:12fb9cbcabcc 23 Kalman();
einsteingustavo 0:12fb9cbcabcc 24
einsteingustavo 0:12fb9cbcabcc 25 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
einsteingustavo 0:12fb9cbcabcc 26 float getAngle(float newAngle, float newRate, float dt);
einsteingustavo 0:12fb9cbcabcc 27
einsteingustavo 0:12fb9cbcabcc 28 void setAngle(float angle); // Used to set angle, this should be set as the starting angle
einsteingustavo 0:12fb9cbcabcc 29 float getRate(); // Return the unbiased rate
einsteingustavo 0:12fb9cbcabcc 30
einsteingustavo 0:12fb9cbcabcc 31 /* These are used to tune the Kalman filter */
einsteingustavo 0:12fb9cbcabcc 32 void setQangle(float Q_angle);
einsteingustavo 0:12fb9cbcabcc 33 /**
einsteingustavo 0:12fb9cbcabcc 34 * setQbias(float Q_bias)
einsteingustavo 0:12fb9cbcabcc 35 * Default value (0.003f) is in Kalman.cpp.
einsteingustavo 0:12fb9cbcabcc 36 * Raise this to follow input more closely,
einsteingustavo 0:12fb9cbcabcc 37 * lower this to smooth result of kalman filter.
einsteingustavo 0:12fb9cbcabcc 38 */
einsteingustavo 0:12fb9cbcabcc 39 void setQbias(float Q_bias);
einsteingustavo 0:12fb9cbcabcc 40 void setRmeasure(float R_measure);
einsteingustavo 0:12fb9cbcabcc 41
einsteingustavo 0:12fb9cbcabcc 42 float getQangle();
einsteingustavo 0:12fb9cbcabcc 43 float getQbias();
einsteingustavo 0:12fb9cbcabcc 44 float getRmeasure();
einsteingustavo 0:12fb9cbcabcc 45
einsteingustavo 0:12fb9cbcabcc 46 private:
einsteingustavo 0:12fb9cbcabcc 47 /* Kalman filter variables */
einsteingustavo 0:12fb9cbcabcc 48 float Q_angle; // Process noise variance for the accelerometer
einsteingustavo 0:12fb9cbcabcc 49 float Q_bias; // Process noise variance for the gyro bias
einsteingustavo 0:12fb9cbcabcc 50 float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
einsteingustavo 0:12fb9cbcabcc 51
einsteingustavo 0:12fb9cbcabcc 52 float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
einsteingustavo 0:12fb9cbcabcc 53 float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
einsteingustavo 0:12fb9cbcabcc 54 float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
einsteingustavo 0:12fb9cbcabcc 55
einsteingustavo 0:12fb9cbcabcc 56 float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
einsteingustavo 0:12fb9cbcabcc 57 };
einsteingustavo 0:12fb9cbcabcc 58
einsteingustavo 0:12fb9cbcabcc 59 #endif
einsteingustavo 0:12fb9cbcabcc 60