User | Revision | Line number | New contents of line |
nimbusgb |
0:6a5296aa7dbf
|
1
|
/**
|
nimbusgb |
0:6a5296aa7dbf
|
2
|
* @author Aaron Berk
|
nimbusgb |
0:6a5296aa7dbf
|
3
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
4
|
* @section LICENSE
|
nimbusgb |
0:6a5296aa7dbf
|
5
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
6
|
* Copyright (c) 2010 ARM Limited
|
nimbusgb |
0:6a5296aa7dbf
|
7
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
8
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
nimbusgb |
0:6a5296aa7dbf
|
9
|
* of this software and associated documentation files (the "Software"), to deal
|
nimbusgb |
0:6a5296aa7dbf
|
10
|
* in the Software without restriction, including without limitation the rights
|
nimbusgb |
0:6a5296aa7dbf
|
11
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
nimbusgb |
0:6a5296aa7dbf
|
12
|
* copies of the Software, and to permit persons to whom the Software is
|
nimbusgb |
0:6a5296aa7dbf
|
13
|
* furnished to do so, subject to the following conditions:
|
nimbusgb |
0:6a5296aa7dbf
|
14
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
15
|
* The above copyright notice and this permission notice shall be included in
|
nimbusgb |
0:6a5296aa7dbf
|
16
|
* all copies or substantial portions of the Software.
|
nimbusgb |
0:6a5296aa7dbf
|
17
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
18
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
nimbusgb |
0:6a5296aa7dbf
|
19
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
nimbusgb |
0:6a5296aa7dbf
|
20
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
nimbusgb |
0:6a5296aa7dbf
|
21
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
nimbusgb |
0:6a5296aa7dbf
|
22
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
nimbusgb |
0:6a5296aa7dbf
|
23
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
nimbusgb |
0:6a5296aa7dbf
|
24
|
* THE SOFTWARE.
|
nimbusgb |
0:6a5296aa7dbf
|
25
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
26
|
* @section DESCRIPTION
|
nimbusgb |
0:6a5296aa7dbf
|
27
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
28
|
* IMU orientation filter developed by Sebastian Madgwick.
|
nimbusgb |
0:6a5296aa7dbf
|
29
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
30
|
* Find more details about his paper here:
|
nimbusgb |
0:6a5296aa7dbf
|
31
|
*
|
nimbusgb |
0:6a5296aa7dbf
|
32
|
* http://code.google.com/p/imumargalgorithm30042010sohm/
|
nimbusgb |
0:6a5296aa7dbf
|
33
|
*/
|
nimbusgb |
0:6a5296aa7dbf
|
34
|
|
nimbusgb |
0:6a5296aa7dbf
|
35
|
/**
|
nimbusgb |
0:6a5296aa7dbf
|
36
|
* Includes
|
nimbusgb |
0:6a5296aa7dbf
|
37
|
*/
|
nimbusgb |
0:6a5296aa7dbf
|
38
|
#include "IMUfilter.h"
|
nimbusgb |
0:6a5296aa7dbf
|
39
|
|
nimbusgb |
0:6a5296aa7dbf
|
40
|
IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){
|
nimbusgb |
0:6a5296aa7dbf
|
41
|
|
nimbusgb |
0:6a5296aa7dbf
|
42
|
firstUpdate = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
43
|
|
nimbusgb |
0:6a5296aa7dbf
|
44
|
//Quaternion orientation of earth frame relative to auxiliary frame.
|
nimbusgb |
0:6a5296aa7dbf
|
45
|
AEq_1 = 1;
|
nimbusgb |
0:6a5296aa7dbf
|
46
|
AEq_2 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
47
|
AEq_3 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
48
|
AEq_4 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
49
|
|
nimbusgb |
0:6a5296aa7dbf
|
50
|
//Estimated orientation quaternion elements with initial conditions.
|
nimbusgb |
0:6a5296aa7dbf
|
51
|
SEq_1 = 1;
|
nimbusgb |
0:6a5296aa7dbf
|
52
|
SEq_2 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
53
|
SEq_3 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
54
|
SEq_4 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
55
|
|
nimbusgb |
0:6a5296aa7dbf
|
56
|
//Sampling period (typical value is ~0.1s).
|
nimbusgb |
0:6a5296aa7dbf
|
57
|
deltat = rate;
|
nimbusgb |
0:6a5296aa7dbf
|
58
|
|
nimbusgb |
0:6a5296aa7dbf
|
59
|
//Gyroscope measurement error (in degrees per second).
|
nimbusgb |
0:6a5296aa7dbf
|
60
|
gyroMeasError = gyroscopeMeasurementError;
|
nimbusgb |
0:6a5296aa7dbf
|
61
|
|
nimbusgb |
0:6a5296aa7dbf
|
62
|
//Compute beta.
|
nimbusgb |
0:6a5296aa7dbf
|
63
|
beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
|
nimbusgb |
0:6a5296aa7dbf
|
64
|
|
nimbusgb |
0:6a5296aa7dbf
|
65
|
}
|
nimbusgb |
0:6a5296aa7dbf
|
66
|
|
nimbusgb |
0:6a5296aa7dbf
|
67
|
void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
|
nimbusgb |
0:6a5296aa7dbf
|
68
|
|
nimbusgb |
0:6a5296aa7dbf
|
69
|
//Local system variables.
|
nimbusgb |
0:6a5296aa7dbf
|
70
|
|
nimbusgb |
0:6a5296aa7dbf
|
71
|
//Vector norm.
|
nimbusgb |
0:6a5296aa7dbf
|
72
|
double norm;
|
nimbusgb |
0:6a5296aa7dbf
|
73
|
//Quaternion rate from gyroscope elements.
|
nimbusgb |
0:6a5296aa7dbf
|
74
|
double SEqDot_omega_1;
|
nimbusgb |
0:6a5296aa7dbf
|
75
|
double SEqDot_omega_2;
|
nimbusgb |
0:6a5296aa7dbf
|
76
|
double SEqDot_omega_3;
|
nimbusgb |
0:6a5296aa7dbf
|
77
|
double SEqDot_omega_4;
|
nimbusgb |
0:6a5296aa7dbf
|
78
|
//Objective function elements.
|
nimbusgb |
0:6a5296aa7dbf
|
79
|
double f_1;
|
nimbusgb |
0:6a5296aa7dbf
|
80
|
double f_2;
|
nimbusgb |
0:6a5296aa7dbf
|
81
|
double f_3;
|
nimbusgb |
0:6a5296aa7dbf
|
82
|
//Objective function Jacobian elements.
|
nimbusgb |
0:6a5296aa7dbf
|
83
|
double J_11or24;
|
nimbusgb |
0:6a5296aa7dbf
|
84
|
double J_12or23;
|
nimbusgb |
0:6a5296aa7dbf
|
85
|
double J_13or22;
|
nimbusgb |
0:6a5296aa7dbf
|
86
|
double J_14or21;
|
nimbusgb |
0:6a5296aa7dbf
|
87
|
double J_32;
|
nimbusgb |
0:6a5296aa7dbf
|
88
|
double J_33;
|
nimbusgb |
0:6a5296aa7dbf
|
89
|
//Objective function gradient elements.
|
nimbusgb |
0:6a5296aa7dbf
|
90
|
double nablaf_1;
|
nimbusgb |
0:6a5296aa7dbf
|
91
|
double nablaf_2;
|
nimbusgb |
0:6a5296aa7dbf
|
92
|
double nablaf_3;
|
nimbusgb |
0:6a5296aa7dbf
|
93
|
double nablaf_4;
|
nimbusgb |
0:6a5296aa7dbf
|
94
|
|
nimbusgb |
0:6a5296aa7dbf
|
95
|
//Auxiliary variables to avoid reapeated calculations.
|
nimbusgb |
0:6a5296aa7dbf
|
96
|
double halfSEq_1 = 0.5 * SEq_1;
|
nimbusgb |
0:6a5296aa7dbf
|
97
|
double halfSEq_2 = 0.5 * SEq_2;
|
nimbusgb |
0:6a5296aa7dbf
|
98
|
double halfSEq_3 = 0.5 * SEq_3;
|
nimbusgb |
0:6a5296aa7dbf
|
99
|
double halfSEq_4 = 0.5 * SEq_4;
|
nimbusgb |
0:6a5296aa7dbf
|
100
|
double twoSEq_1 = 2.0 * SEq_1;
|
nimbusgb |
0:6a5296aa7dbf
|
101
|
double twoSEq_2 = 2.0 * SEq_2;
|
nimbusgb |
0:6a5296aa7dbf
|
102
|
double twoSEq_3 = 2.0 * SEq_3;
|
nimbusgb |
0:6a5296aa7dbf
|
103
|
|
nimbusgb |
0:6a5296aa7dbf
|
104
|
//Compute the quaternion rate measured by gyroscopes.
|
nimbusgb |
0:6a5296aa7dbf
|
105
|
SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
|
nimbusgb |
0:6a5296aa7dbf
|
106
|
SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
|
nimbusgb |
0:6a5296aa7dbf
|
107
|
SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
|
nimbusgb |
0:6a5296aa7dbf
|
108
|
SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
|
nimbusgb |
0:6a5296aa7dbf
|
109
|
|
nimbusgb |
0:6a5296aa7dbf
|
110
|
//Normalise the accelerometer measurement.
|
nimbusgb |
0:6a5296aa7dbf
|
111
|
norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
|
nimbusgb |
0:6a5296aa7dbf
|
112
|
a_x /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
113
|
a_y /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
114
|
a_z /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
115
|
|
nimbusgb |
0:6a5296aa7dbf
|
116
|
//Compute the objective function and Jacobian.
|
nimbusgb |
0:6a5296aa7dbf
|
117
|
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
|
nimbusgb |
0:6a5296aa7dbf
|
118
|
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
|
nimbusgb |
0:6a5296aa7dbf
|
119
|
f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
|
nimbusgb |
0:6a5296aa7dbf
|
120
|
//J_11 negated in matrix multiplication.
|
nimbusgb |
0:6a5296aa7dbf
|
121
|
J_11or24 = twoSEq_3;
|
nimbusgb |
0:6a5296aa7dbf
|
122
|
J_12or23 = 2 * SEq_4;
|
nimbusgb |
0:6a5296aa7dbf
|
123
|
//J_12 negated in matrix multiplication
|
nimbusgb |
0:6a5296aa7dbf
|
124
|
J_13or22 = twoSEq_1;
|
nimbusgb |
0:6a5296aa7dbf
|
125
|
J_14or21 = twoSEq_2;
|
nimbusgb |
0:6a5296aa7dbf
|
126
|
//Negated in matrix multiplication.
|
nimbusgb |
0:6a5296aa7dbf
|
127
|
J_32 = 2 * J_14or21;
|
nimbusgb |
0:6a5296aa7dbf
|
128
|
//Negated in matrix multiplication.
|
nimbusgb |
0:6a5296aa7dbf
|
129
|
J_33 = 2 * J_11or24;
|
nimbusgb |
0:6a5296aa7dbf
|
130
|
|
nimbusgb |
0:6a5296aa7dbf
|
131
|
//Compute the gradient (matrix multiplication).
|
nimbusgb |
0:6a5296aa7dbf
|
132
|
nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
|
nimbusgb |
0:6a5296aa7dbf
|
133
|
nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
|
nimbusgb |
0:6a5296aa7dbf
|
134
|
nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
|
nimbusgb |
0:6a5296aa7dbf
|
135
|
nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
|
nimbusgb |
0:6a5296aa7dbf
|
136
|
|
nimbusgb |
0:6a5296aa7dbf
|
137
|
//Normalise the gradient.
|
nimbusgb |
0:6a5296aa7dbf
|
138
|
norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
|
nimbusgb |
0:6a5296aa7dbf
|
139
|
nablaf_1 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
140
|
nablaf_2 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
141
|
nablaf_3 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
142
|
nablaf_4 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
143
|
|
nimbusgb |
0:6a5296aa7dbf
|
144
|
//Compute then integrate the estimated quaternion rate.
|
nimbusgb |
0:6a5296aa7dbf
|
145
|
SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
|
nimbusgb |
0:6a5296aa7dbf
|
146
|
SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
|
nimbusgb |
0:6a5296aa7dbf
|
147
|
SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
|
nimbusgb |
0:6a5296aa7dbf
|
148
|
SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
|
nimbusgb |
2:202906c5fadd
|
149
|
|
nimbusgb |
0:6a5296aa7dbf
|
150
|
//Normalise quaternion
|
nimbusgb |
0:6a5296aa7dbf
|
151
|
norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
|
nimbusgb |
0:6a5296aa7dbf
|
152
|
SEq_1 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
153
|
SEq_2 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
154
|
SEq_3 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
155
|
SEq_4 /= norm;
|
nimbusgb |
0:6a5296aa7dbf
|
156
|
|
nimbusgb |
0:6a5296aa7dbf
|
157
|
if (firstUpdate == 0) {
|
nimbusgb |
0:6a5296aa7dbf
|
158
|
//Store orientation of auxiliary frame.
|
nimbusgb |
0:6a5296aa7dbf
|
159
|
AEq_1 = SEq_1;
|
nimbusgb |
0:6a5296aa7dbf
|
160
|
AEq_2 = SEq_2;
|
nimbusgb |
0:6a5296aa7dbf
|
161
|
AEq_3 = SEq_3;
|
nimbusgb |
0:6a5296aa7dbf
|
162
|
AEq_4 = SEq_4;
|
nimbusgb |
0:6a5296aa7dbf
|
163
|
firstUpdate = 1;
|
nimbusgb |
0:6a5296aa7dbf
|
164
|
}
|
nimbusgb |
0:6a5296aa7dbf
|
165
|
|
nimbusgb |
0:6a5296aa7dbf
|
166
|
}
|
nimbusgb |
0:6a5296aa7dbf
|
167
|
|
nimbusgb |
0:6a5296aa7dbf
|
168
|
void IMUfilter::computeEuler(void){
|
nimbusgb |
0:6a5296aa7dbf
|
169
|
|
nimbusgb |
0:6a5296aa7dbf
|
170
|
//Quaternion describing orientation of sensor relative to earth.
|
nimbusgb |
0:6a5296aa7dbf
|
171
|
double ESq_1, ESq_2, ESq_3, ESq_4;
|
nimbusgb |
0:6a5296aa7dbf
|
172
|
//Quaternion describing orientation of sensor relative to auxiliary frame.
|
nimbusgb |
0:6a5296aa7dbf
|
173
|
double ASq_1, ASq_2, ASq_3, ASq_4;
|
nimbusgb |
0:6a5296aa7dbf
|
174
|
|
nimbusgb |
0:6a5296aa7dbf
|
175
|
//Compute the quaternion conjugate.
|
nimbusgb |
1:4b0e8441f099
|
176
|
ESq_1 = SEq_1;
|
nimbusgb |
0:6a5296aa7dbf
|
177
|
ESq_2 = -SEq_2;
|
nimbusgb |
0:6a5296aa7dbf
|
178
|
ESq_3 = -SEq_3;
|
nimbusgb |
0:6a5296aa7dbf
|
179
|
ESq_4 = -SEq_4;
|
nimbusgb |
0:6a5296aa7dbf
|
180
|
|
nimbusgb |
0:6a5296aa7dbf
|
181
|
//Compute the quaternion product.
|
nimbusgb |
0:6a5296aa7dbf
|
182
|
ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
|
nimbusgb |
0:6a5296aa7dbf
|
183
|
ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
|
nimbusgb |
0:6a5296aa7dbf
|
184
|
ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
|
nimbusgb |
0:6a5296aa7dbf
|
185
|
ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
|
nimbusgb |
0:6a5296aa7dbf
|
186
|
|
nimbusgb |
0:6a5296aa7dbf
|
187
|
//Compute the Euler angles from the quaternion.
|
nimbusgb |
1:4b0e8441f099
|
188
|
phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
|
nimbusgb |
1:4b0e8441f099
|
189
|
theta = asin (2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
|
nimbusgb |
1:4b0e8441f099
|
190
|
psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
|
nimbusgb |
0:6a5296aa7dbf
|
191
|
|
nimbusgb |
0:6a5296aa7dbf
|
192
|
}
|
nimbusgb |
0:6a5296aa7dbf
|
193
|
|
nimbusgb |
0:6a5296aa7dbf
|
194
|
double IMUfilter::getRoll(void){
|
nimbusgb |
0:6a5296aa7dbf
|
195
|
|
nimbusgb |
0:6a5296aa7dbf
|
196
|
return phi;
|
nimbusgb |
0:6a5296aa7dbf
|
197
|
|
nimbusgb |
0:6a5296aa7dbf
|
198
|
}
|
nimbusgb |
0:6a5296aa7dbf
|
199
|
|
nimbusgb |
0:6a5296aa7dbf
|
200
|
double IMUfilter::getPitch(void){
|
nimbusgb |
0:6a5296aa7dbf
|
201
|
|
nimbusgb |
0:6a5296aa7dbf
|
202
|
return theta;
|
nimbusgb |
0:6a5296aa7dbf
|
203
|
|
nimbusgb |
0:6a5296aa7dbf
|
204
|
}
|
nimbusgb |
0:6a5296aa7dbf
|
205
|
|
nimbusgb |
0:6a5296aa7dbf
|
206
|
double IMUfilter::getYaw(void){
|
nimbusgb |
0:6a5296aa7dbf
|
207
|
|
nimbusgb |
0:6a5296aa7dbf
|
208
|
return psi;
|
nimbusgb |
0:6a5296aa7dbf
|
209
|
|
nimbusgb |
0:6a5296aa7dbf
|
210
|
}
|
nimbusgb |
0:6a5296aa7dbf
|
211
|
|
nimbusgb |
1:4b0e8441f099
|
212
|
void IMUfilter::setGyroError(double gyroscopeMeasurementError)
|
nimbusgb |
1:4b0e8441f099
|
213
|
{
|
nimbusgb |
1:4b0e8441f099
|
214
|
gyroMeasError = gyroscopeMeasurementError;
|
nimbusgb |
1:4b0e8441f099
|
215
|
reset();
|
nimbusgb |
1:4b0e8441f099
|
216
|
}
|
nimbusgb |
1:4b0e8441f099
|
217
|
|
nimbusgb |
1:4b0e8441f099
|
218
|
double IMUfilter::getGyroError(void)
|
nimbusgb |
1:4b0e8441f099
|
219
|
{
|
nimbusgb |
1:4b0e8441f099
|
220
|
return gyroMeasError;
|
nimbusgb |
1:4b0e8441f099
|
221
|
}
|
nimbusgb |
1:4b0e8441f099
|
222
|
|
nimbusgb |
0:6a5296aa7dbf
|
223
|
void IMUfilter::reset(void) {
|
nimbusgb |
0:6a5296aa7dbf
|
224
|
|
nimbusgb |
0:6a5296aa7dbf
|
225
|
firstUpdate = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
226
|
|
nimbusgb |
0:6a5296aa7dbf
|
227
|
//Quaternion orientation of earth frame relative to auxiliary frame.
|
nimbusgb |
0:6a5296aa7dbf
|
228
|
AEq_1 = 1;
|
nimbusgb |
0:6a5296aa7dbf
|
229
|
AEq_2 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
230
|
AEq_3 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
231
|
AEq_4 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
232
|
|
nimbusgb |
0:6a5296aa7dbf
|
233
|
//Estimated orientation quaternion elements with initial conditions.
|
nimbusgb |
0:6a5296aa7dbf
|
234
|
SEq_1 = 1;
|
nimbusgb |
0:6a5296aa7dbf
|
235
|
SEq_2 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
236
|
SEq_3 = 0;
|
nimbusgb |
0:6a5296aa7dbf
|
237
|
SEq_4 = 0;
|
nimbusgb |
1:4b0e8441f099
|
238
|
|
nimbusgb |
1:4b0e8441f099
|
239
|
//Compute beta.
|
nimbusgb |
1:4b0e8441f099
|
240
|
beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
|
nimbusgb |
0:6a5296aa7dbf
|
241
|
}
|