-
Notifications
You must be signed in to change notification settings - Fork 3
/
kalman_filter.h
79 lines (69 loc) · 2.3 KB
/
kalman_filter.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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
/****************************************************************************
* Copyright (C) 2018 RoboMaster.
*
* This program is free software: you can redistribute it and/or modify
* it 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.
*
* This program is distributed in the hope that it 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 program. If not, see <http://www.gnu.org/licenses/>.
***************************************************************************/
/** @file kalman_filter.h
* @version 1.0
* @date Apr 2018
*
* @brief kalman filter realization
*
* @copyright 2018 DJI RoboMaster. All rights reserved.
*
*/
#ifndef __KALMAN_FILTER_H__
#define __KALMAN_FILTER_H__
#include "stm32f4xx_hal.h"
#include "arm_math.h"
#define mat arm_matrix_instance_f32
#define mat_64 arm_matrix_instance_f64
#define mat_init arm_mat_init_f32
#define mat_add arm_mat_add_f32
#define mat_sub arm_mat_sub_f32
#define mat_mult arm_mat_mult_f32
#define mat_trans arm_mat_trans_f32
#define mat_inv arm_mat_inverse_f32
#define mat_inv_f64 arm_mat_inverse_f64
typedef struct
{
float raw_value;
float filtered_value[2];
mat xhat, xhatminus, z, A, H, AT, HT, Q, R, P, Pminus, K;
} kalman_filter_t;
typedef struct
{
float raw_value;
float filtered_value[2];
float xhat_data[2], xhatminus_data[2], z_data[2],Pminus_data[4], K_data[4];
float P_data[4];
float AT_data[4], HT_data[4];
float A_data[4];
float H_data[4];
float Q_data[4];
float R_data[4];
} kalman_filter_init_t;
typedef struct
{
int delay_cnt;
int freq;
int last_time;
float last_position;
float speed;
float last_speed;
float processed_speed;
} speed_calc_data_t;
void kalman_filter_init(kalman_filter_t *F, kalman_filter_init_t *I); //把float数组init为mat
float *kalman_filter_calc(kalman_filter_t *F, float signal1, float signal2);
#endif