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
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
|
/*
* kalman.c
*
* Created on: 01.12.2010
* Author: Laurens Mackay
*/
#include "kalman.h"
//#include "mavlink_debug.h"
void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[],
m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[],
m_elem x_aposteriori[], int gainfactorsteps)
{
kalman->states = states;
kalman->measurements = measurements;
kalman->gainfactorsteps = gainfactorsteps;
kalman->gainfactor = 0;
//Create all matrices that are persistent
kalman->a = matrix_create(states, states, a);
kalman->c = matrix_create(measurements, states, c);
kalman->gain_start = matrix_create(states, measurements, gain_start);
kalman->gain = matrix_create(states, measurements, gain);
kalman->x_apriori = matrix_create(states, 1, x_apriori);
kalman->x_aposteriori = matrix_create(states, 1, x_aposteriori);
}
void kalman_predict(kalman_t *kalman)
{
matrix_mult(kalman->a, kalman->x_aposteriori, kalman->x_apriori);
}
void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[])
{
//create matrices from inputs
matrix_t measurement =
matrix_create(kalman->measurements, 1, measurement_a);
matrix_t mask = matrix_create(kalman->measurements, 1, mask_a);
//create temporary matrices
m_elem gain_start_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] =
{ };
matrix_t gain_start_part = matrix_create(kalman->states,
kalman->measurements, gain_start_part_a);
m_elem gain_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] =
{ };
matrix_t gain_part = matrix_create(kalman->states, kalman->measurements,
gain_part_a);
m_elem gain_sum_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] =
{ };
matrix_t gain_sum = matrix_create(kalman->states, kalman->measurements,
gain_sum_a);
m_elem error_a[KALMAN_MAX_MEASUREMENTS * 1] =
{ };
matrix_t error = matrix_create(kalman->measurements, 1, error_a);
m_elem measurement_estimate_a[KALMAN_MAX_MEASUREMENTS * 1] =
{ };
matrix_t measurement_estimate = matrix_create(kalman->measurements, 1,
measurement_estimate_a);
m_elem x_update_a[KALMAN_MAX_STATES * 1] =
{ };
matrix_t x_update = matrix_create(kalman->states, 1, x_update_a);
//x(:,i+1)=xapriori+(gainfactor*[M_50(:,1) M(:,2)]+(1-gainfactor)*M_start)*(z-C*xapriori);
//est=C*xapriori;
matrix_mult(kalman->c, kalman->x_apriori, measurement_estimate);
//error=(z-C*xapriori) = measurement-estimate
matrix_sub(measurement, measurement_estimate, error);
matrix_mult_element(error, mask, error);
kalman->gainfactor = kalman->gainfactor * (1.0f - 1.0f
/ kalman->gainfactorsteps) + 1.0f * 1.0f / kalman->gainfactorsteps;
matrix_mult_scalar(kalman->gainfactor, kalman->gain, gain_part);
matrix_mult_scalar(1.0f - kalman->gainfactor, kalman->gain_start,
gain_start_part);
matrix_add(gain_start_part, gain_part, gain_sum);
//gain*(z-C*xapriori)
matrix_mult(gain_sum, error, x_update);
//xaposteriori = xapriori + update
matrix_add(kalman->x_apriori, x_update, kalman->x_aposteriori);
// static int i=0;
// if(i++==4){
// i=0;
// float_vect3 out_kal;
// out_kal.x = M(gain_sum,0,1);
//// out_kal_z.x = z_measurement[1];
// out_kal.y = M(gain_sum,1,1);
// out_kal.z = M(gain_sum,2,1);
// debug_vect("out_kal", out_kal);
// }
}
m_elem kalman_get_state(kalman_t *kalman, int state)
{
return M(kalman->x_aposteriori, state, 0);
}
|
