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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
|
/*
* positionKalmanFilter1D.c
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
real32_T P_aposteriori[9])
{
int32_T i0;
real32_T f0;
int32_T k;
real32_T b_A[9];
int32_T i1;
real32_T P_apriori[9];
real32_T y;
real32_T K[3];
real32_T S;
int8_T I[9];
/* prediction */
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
}
x_aposteriori[i0] = f0 + B[i0] * u;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
}
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
}
P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
}
}
if ((real32_T)fabs(u) < thresh) {
x_aposteriori[1] *= decay;
}
/* update */
if (gps_update == 1) {
y = 0.0F;
for (k = 0; k < 3; k++) {
y += C[k] * x_aposteriori[k];
K[k] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
K[k] += C[i0] * P_apriori[i0 + 3 * k];
}
}
y = z - y;
S = 0.0F;
for (k = 0; k < 3; k++) {
S += K[k] * C[k];
}
S += R;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += P_apriori[i0 + 3 * k] * C[k];
}
K[i0] = f0 / S;
}
for (i0 = 0; i0 < 3; i0++) {
x_aposteriori[i0] += K[i0] * y;
}
for (i0 = 0; i0 < 9; i0++) {
I[i0] = 0;
}
for (k = 0; k < 3; k++) {
I[k + 3 * k] = 1;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
P_aposteriori[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
}
}
}
} else {
for (i0 = 0; i0 < 9; i0++) {
P_aposteriori[i0] = P_apriori[i0];
}
}
}
/* End of code generation (positionKalmanFilter1D.c) */
|
