Sensor Scol plugin
Multi platform sensors for handled devices
orientationEKF.cpp
1
2#include "tools/OrientationEKF.h"
3#include "tools/SO3Util.h"
4
5#include <cmath>
6#include <algorithm>
7
8OrientationEKF::OrientationEKF()
9{
10 reset();
11}
12
13OrientationEKF::~OrientationEKF()
14{
15}
16
17void OrientationEKF::reset()
18{
19 timestepFilterInit = false;
20 gyroFilterValid = true;
21 alignedToGravity = false;
22
23 sensorTimeStampGyro = 0;
24 sensorTimeStampMag = 0;
25 previousAccelNorm = 0.0;
26 movingAverageAccelNormChange = 0.0;
27
28 so3SensorFromWorld.setIdentity();
29 so3LastMotion.setIdentity();
30
31 mP.setZero();
32 mP.setDiagonal(25.0);
33
34 mQ.setZero();
35 mQ.setDiagonal(1.0);
36
37 mR.setZero();
38 mR.setDiagonal(0.0625);
39
40 mRaccel.setZero();
41 mRaccel.setDiagonal(0.5625);
42
43 mS.setZero();
44 mH.setZero();
45 mK.setZero();
46 mNu.setZero();
47 mz.setZero();
48 mh.setZero();
49 mu.setZero();
50 mx.setZero();
51
52 down.set(0.0, 0.0, EARTH_GRAVITY);
53 north.set(0.0, 1.0, 0.0);
54}
55
56bool OrientationEKF::isReady()
57{
58 return alignedToGravity;
59}
60
61double OrientationEKF::getHeadingDegrees()
62{
63 double x = so3SensorFromWorld.get(2, 0);
64 double y = so3SensorFromWorld.get(2, 1);
65 double mag = sqrt(x * x + y * y);
66
67 if (mag < 0.1)
68 {
69 return 0.0;
70 }
71
72 double heading = -90.0 - atan2(y, x) * RAD_TO_DEG;
73 if (heading < 0.0)
74 {
75 heading += 360.0;
76 }
77 if (heading >= 360.0)
78 {
79 heading -= 360.0;
80 }
81 return heading;
82}
83
84void OrientationEKF::setHeadingDegrees(double heading)
85{
86 double currentHeading = getHeadingDegrees();
87 double deltaHeading = heading - currentHeading;
88 double s = sin(deltaHeading * DEG_TO_RAD);
89 double c = cos(deltaHeading * DEG_TO_RAD);
90
91 Matrix3x3d setHeadingDegreesTempM1(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0);
92 Matrix3x3d::mult(so3SensorFromWorld, setHeadingDegreesTempM1, so3SensorFromWorld);
93}
94
95void OrientationEKF::processGyro(Vector3d gyro, int64_t sensorTimeStamp)
96{
97 double kTimeThreshold = 0.04;
98 double kdTdefault = 0.01;
99 if (sensorTimeStampGyro != 0)
100 {
101 double dT = (double)(sensorTimeStamp - sensorTimeStampGyro) * 1.e-09;
102 if (dT > kTimeThreshold)
103 dT = gyroFilterValid ? filteredGyroTimestep : kdTdefault;
104 else
105 filterGyroTimestep(dT);
106
107 mu.set(gyro.x * -dT, gyro.y * -dT, gyro.z * -dT);
108 SO3Util::sO3FromMu(mu, so3LastMotion);
109
110 Matrix3x3d processGyroTempM1;
111 processGyroTempM1.set(so3SensorFromWorld);
112 Matrix3x3d::mult(so3LastMotion, so3SensorFromWorld, processGyroTempM1);
113 so3SensorFromWorld.set(processGyroTempM1);
114
115 updateCovariancesAfterMotion();
116
117 Matrix3x3d processGyroTempM2;
118 processGyroTempM2.set(mQ);
119 processGyroTempM2.scale(dT * dT);
120 mP.plusEquals(processGyroTempM2);
121 }
122 sensorTimeStampGyro = sensorTimeStamp;
123 lastGyro = gyro;
124}
125
126void OrientationEKF::processAcc(Vector3d acc, int64_t sensorTimeStamp)
127{
128 mz.set(acc.x, acc.y, acc.z);
129 updateAccelerationCovariance(mz.length());
130 if (alignedToGravity)
131 {
132 accObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
133
134 double eps = 1.0E-07;
135 for (int dof = 0; dof < 3; dof++)
136 {
137 Vector3d delta;
138 delta.setZero();
139 delta.setComponent(dof, eps);
140
141 Matrix3x3d processAcc1;
142 Matrix3x3d processAcc2;
143 SO3Util::sO3FromMu(delta, processAcc1);
144 Matrix3x3d::mult(processAcc1, so3SensorFromWorld, processAcc2);
145
146 Vector3d withDelta;
147 accObservationFunctionForNumericalJacobian(processAcc2, withDelta);
148
149 Vector3d processAccV;
150 Vector3d::sub(mNu, withDelta, processAccV);
151 processAccV.scale(1.0 / eps);
152 mH.setColumn(dof, processAccV);
153 }
154
155 Matrix3x3d processAccTempM3;
156 Matrix3x3d processAccTempM4;
157 Matrix3x3d processAccTempM5;
158 mH.transpose(processAccTempM3);
159 Matrix3x3d::mult(mP, processAccTempM3, processAccTempM4);
160 Matrix3x3d::mult(mH, processAccTempM4, processAccTempM5);
161 Matrix3x3d::add(processAccTempM5, mRaccel, mS);
162
163 mS.invert(processAccTempM3);
164 mH.transpose(processAccTempM4);
165 Matrix3x3d::mult(processAccTempM4, processAccTempM3, processAccTempM5);
166 Matrix3x3d::mult(mP, processAccTempM5, mK);
167
168 Matrix3x3d::mult(mK, mNu, mx);
169
170 Matrix3x3d::mult(mK, mH, processAccTempM3);
171 processAccTempM4.setIdentity();
172 processAccTempM4.minusEquals(processAccTempM3);
173 Matrix3x3d::mult(processAccTempM4, mP, processAccTempM3);
174 mP.set(processAccTempM3);
175
176 SO3Util::sO3FromMu(mx, so3LastMotion);
177
178 Matrix3x3d::mult(so3LastMotion, so3SensorFromWorld, so3SensorFromWorld);
179
180 updateCovariancesAfterMotion();
181 }
182 else
183 {
184 SO3Util::sO3FromTwoVec(down, mz, so3SensorFromWorld);
185 alignedToGravity = true;
186 }
187}
188
189void OrientationEKF::processMag(Vector3d mag, int64_t sensorTimeStamp)
190{
191 mz.set(mag.x, mag.y, mag.z);
192 mz.normalize();
193
194 Vector3d downInSensorFrame;
195 so3SensorFromWorld.getColumn(2, downInSensorFrame);
196
197 Vector3d perpToDownAndMag;
198 Vector3d::cross(mz, downInSensorFrame, perpToDownAndMag);
199 perpToDownAndMag.normalize();
200
201 Vector3d magHorizontal;
202 Vector3d::cross(downInSensorFrame, perpToDownAndMag, magHorizontal);
203
204 magHorizontal.normalize();
205 mz.set(magHorizontal);
206
207 Matrix3x3d processMagTempM1;
208 Matrix3x3d processMagTempM2;
209 Matrix3x3d processMagTempM4;
210 Matrix3x3d processMagTempM5;
211 Matrix3x3d processMagTempM6;
212
213 if (sensorTimeStampMag != 0)
214 {
215 magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
216
217 Vector3d processMagTempV;
218 double eps = 1.0E-07;
219
220 for (int dof = 0; dof < 3; dof++)
221 {
222 Vector3d delta;
223 delta.setZero();
224 delta.setComponent(dof, eps);
225
226 SO3Util::sO3FromMu(delta, processMagTempM1);
227 Matrix3x3d::mult(processMagTempM1, so3SensorFromWorld, processMagTempM2);
228
229 Vector3d withDelta;
230 magObservationFunctionForNumericalJacobian(processMagTempM2, withDelta);
231
232 Vector3d::sub(mNu, withDelta, processMagTempV);
233 processMagTempV.scale(1.0 / eps);
234
235 mH.setColumn(dof, processMagTempV);
236 }
237
238 mH.transpose(processMagTempM4);
239 Matrix3x3d::mult(mP, processMagTempM4, processMagTempM5);
240 Matrix3x3d::mult(mH, processMagTempM5, processMagTempM6);
241 Matrix3x3d::add(processMagTempM6, mR, mS);
242
243 mS.invert(processMagTempM4);
244 mH.transpose(processMagTempM5);
245 Matrix3x3d::mult(processMagTempM5, processMagTempM4, processMagTempM6);
246 Matrix3x3d::mult(mP, processMagTempM6, mK);
247
248 Matrix3x3d::mult(mK, mNu, mx);
249 Matrix3x3d::mult(mK, mH, processMagTempM4);
250 processMagTempM5.setIdentity();
251 processMagTempM5.minusEquals(processMagTempM4);
252 Matrix3x3d::mult(processMagTempM5, mP, processMagTempM4);
253 mP.set(processMagTempM4);
254
255 SO3Util::sO3FromMu(mx, so3LastMotion);
256
257 Matrix3x3d::mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
258 so3SensorFromWorld.set(processMagTempM4);
259
260 updateCovariancesAfterMotion();
261 }
262 else
263 {
264 magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
265 SO3Util::sO3FromMu(mx, so3LastMotion);
266
267 Matrix3x3d::mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
268 so3SensorFromWorld.set(processMagTempM4);
269
270 updateCovariancesAfterMotion();
271 }
272 sensorTimeStampMag = sensorTimeStamp;
273}
274
275void OrientationEKF::filterGyroTimestep(double timestep)
276{
277 float kFilterCoeff = 0.95;
278 float kMinSamples = 10.0;
279 if (!timestepFilterInit)
280 {
281 filteredGyroTimestep = timestep;
282 numGyroTimestepSamples = 1;
283 timestepFilterInit = true;
284 }
285 else
286 {
287 filteredGyroTimestep = (kFilterCoeff * filteredGyroTimestep + (1.0 - kFilterCoeff) * timestep);
288 if (++numGyroTimestepSamples > kMinSamples)
289 gyroFilterValid = true;
290 }
291}
292
293void OrientationEKF::updateCovariancesAfterMotion()
294{
295 so3LastMotion.transpose(updateCovariancesAfterMotionTempM1);
296 Matrix3x3d::mult(mP, updateCovariancesAfterMotionTempM1, updateCovariancesAfterMotionTempM2);
297
298 Matrix3x3d::mult(so3LastMotion, updateCovariancesAfterMotionTempM2, mP);
299 so3LastMotion.setIdentity();
300}
301
302void OrientationEKF::accObservationFunctionForNumericalJacobian(Matrix3x3d so3SensorFromWorldPred, Vector3d &result)
303{
304 Matrix3x3d::mult(so3SensorFromWorldPred, down, mh);
305 SO3Util::sO3FromTwoVec(mh, mz, accObservationFunctionForNumericalJacobianTempM);
306
307 SO3Util::muFromSO3(accObservationFunctionForNumericalJacobianTempM, result);
308}
309
310void OrientationEKF::magObservationFunctionForNumericalJacobian(Matrix3x3d so3SensorFromWorldPred, Vector3d &result)
311{
312 Matrix3x3d::mult(so3SensorFromWorldPred, north, mh);
313 SO3Util::sO3FromTwoVec(mh, mz, magObservationFunctionForNumericalJacobianTempM);
314
315 SO3Util::muFromSO3(magObservationFunctionForNumericalJacobianTempM, result);
316}
317
318void OrientationEKF::updateAccelerationCovariance(double currentAccelNorm)
319{
320 double currentAccelNormChange = fabs(currentAccelNorm - previousAccelNorm);
321 previousAccelNorm = currentAccelNorm;
322 const double kSmoothingFactor = 0.5;
323 movingAverageAccelNormChange = kSmoothingFactor * movingAverageAccelNormChange + (1.0 - kSmoothingFactor) * currentAccelNormChange;
324 const double kMaxAccelNormChange = 0.15;
325 const double kMinAccelNoiseSigma = 0.75;
326 const double kMaxAccelNoiseSigma = 7.0;
327 double normChangeRatio = movingAverageAccelNormChange / kMaxAccelNormChange;
328 double accelNoiseSigma = std::min(kMaxAccelNoiseSigma, kMinAccelNoiseSigma + normChangeRatio * (kMaxAccelNoiseSigma - kMinAccelNoiseSigma));
329 mRaccel.setDiagonal(accelNoiseSigma * accelNoiseSigma);
330}
331
332double* OrientationEKF::getMatrix()
333{
334 return MatrixFromSo3(so3SensorFromWorld);
335}
336
337double* OrientationEKF::getPredictedMatrix(double secondsAfterLastGyroEvent)
338{
339 double dT = secondsAfterLastGyroEvent;
340 Vector3d pmu;
341 pmu.set(lastGyro.x * -dT, lastGyro.y * -dT, lastGyro.z * -dT);
342 Matrix3x3d so3PredictedMotion;
343 SO3Util::sO3FromMu(pmu, so3PredictedMotion);
344
345 Matrix3x3d so3PredictedState;
346 Matrix3x3d::mult(so3PredictedMotion, so3SensorFromWorld, so3PredictedState);
347
348 return MatrixFromSo3(so3PredictedState);
349}
350
351double* OrientationEKF::MatrixFromSo3(Matrix3x3d so3)
352{
353 for (int r = 0; r < 3; r++)
354 {
355 for (int c = 0; c < 3; c++)
356 {
357 rotationMatrix[(4 * c + r)] = so3.get(r, c);
358 }
359 }
360 double tmp62_61 = (rotationMatrix[11] = 0.0); rotationMatrix[7] = tmp62_61; rotationMatrix[3] = tmp62_61;
361 double tmp86_85 = (rotationMatrix[14] = 0.0); rotationMatrix[13] = tmp86_85; rotationMatrix[12] = tmp86_85;
362
363 rotationMatrix[15] = 1.0;
364
365 return rotationMatrix;
366}
Generated by doxygen 1.9.8