删除文件 arduino/Betas/RGB_V1.1.1/Kalman.h
parent
b257c62667
commit
2ea9b1453a
|
@ -1,59 +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
|
|
||||||
*/
|
|
||||||
|
|
||||||
#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
|
|
Loading…
Reference in New Issue