-
Notifications
You must be signed in to change notification settings - Fork 1.4k
/
kalman.h
executable file
·54 lines (47 loc) · 1.54 KB
/
kalman.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "sensors/gyro.h"
#include "common/filter.h"
#define MAX_KALMAN_WINDOW_SIZE 512
#define VARIANCE_SCALE 0.67f
typedef struct kalman
{
float q; //process noise covariance
float r; //measurement noise covariance
float p; //estimation error covariance matrix
float k; //kalman gain
float x; //state
float lastX; //previous state
float e;
float s;
float axisVar;
uint16_t windex;
float axisWindow[MAX_KALMAN_WINDOW_SIZE];
float varianceWindow[MAX_KALMAN_WINDOW_SIZE];
float axisSumMean;
float axisMean;
float axisSumVar;
float inverseN;
uint16_t w;
} kalman_t;
void gyroKalmanInitialize(void);
float gyroKalmanUpdate(uint8_t axis, float input);
void gyroKalmanSetSetpoint(uint8_t axis, float rate);