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
|
/*
* kalman_dlqe2.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
real32_T x_aposteriori_k[3], real32_T z, real32_T
x_aposteriori[3])
{
//printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
//printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
real32_T A[9];
real32_T y;
int32_T i0;
static const int8_T iv0[3] = { 0, 0, 1 };
real32_T b_k1[3];
int32_T i1;
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T f0;
A[0] = 1.0F;
A[3] = dt;
A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = dt;
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
A[2 + 3 * i0] = (real32_T)iv0[i0];
b_k1[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
}
y += b_k1[i0] * x_aposteriori_k[i0];
}
y = z - y;
b_k1[0] = k1;
b_k1[1] = k2;
b_k1[2] = k3;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + b_k1[i0] * y;
}
}
/* End of code generation (kalman_dlqe2.c) */
|
