stateless_orientation.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010, CCNY Robotics Lab
3  * Ivan Dryanovski <ivan.dryanovski@gmail.com>
4  *
5  * http://robotics.ccny.cuny.edu
6  *
7  * Based on implementation of Madgwick's IMU and AHRS algorithms.
8  * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
9  *
10  *
11  * This program is free software: you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation, either version 3 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program. If not, see <http://www.gnu.org/licenses/>.
23  */
24 
27 #include <tf2/convert.h>
29 
30 template<typename T>
31 static inline void crossProduct(
32  T ax, T ay, T az,
33  T bx, T by, T bz,
34  T& rx, T& ry, T& rz) {
35  rx = ay*bz - az*by;
36  ry = az*bx - ax*bz;
37  rz = ax*by - ay*bx;
38 }
39 
40 
41 template<typename T>
42 static inline T normalizeVector(T& vx, T& vy, T& vz) {
43  T norm = sqrt(vx*vx + vy*vy + vz*vz);
44  T inv = 1.0 / norm;
45  vx *= inv;
46  vy *= inv;
47  vz *= inv;
48  return norm;
49 
50 }
51 
52 
55  geometry_msgs::Vector3 A,
56  geometry_msgs::Vector3 E,
57  geometry_msgs::Quaternion& orientation) {
58 
59  float Hx, Hy, Hz;
60  float Mx, My, Mz;
61  float normH;
62 
63  // A: pointing up
64  float Ax = A.x, Ay = A.y, Az = A.z;
65 
66  // E: pointing down/north
67  float Ex = E.x, Ey = E.y, Ez = E.z;
68 
69  // H: vector horizontal, pointing east
70  // H = E x A
71  crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz);
72 
73  // normalize H
74  normH = normalizeVector(Hx, Hy, Hz);
75  if (normH < 1E-7) {
76  // device is close to free fall (or in space?), or close to
77  // magnetic north pole.
78  // mag in T => Threshold 1E-7, typical values are > 1E-5.
79  return false;
80  }
81 
82  // normalize A
83  normalizeVector(Ax, Ay, Az);
84 
85  // M: vector horizontal, pointing north
86  // M = A x H
87  crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz);
88 
89  // Create matrix for basis transformation
91  switch (frame) {
92  case WorldFrame::NED:
93  // vector space world W:
94  // Basis: bwx (1,0,0) north, bwy (0,1,0) east, bwz (0,0,1) down
95  // vector space local L:
96  // Basis: M ,H, -A
97  // W(1,0,0) => L(M)
98  // W(0,1,0) => L(H)
99  // W(0,0,1) => L(-A)
100 
101  // R: Transform Matrix local => world equals basis of L, because basis of W is I
102  R[0][0] = Mx; R[0][1] = Hx; R[0][2] = -Ax;
103  R[1][0] = My; R[1][1] = Hy; R[1][2] = -Ay;
104  R[2][0] = Mz; R[2][1] = Hz; R[2][2] = -Az;
105  break;
106 
107  case WorldFrame::NWU:
108  // vector space world W:
109  // Basis: bwx (1,0,0) north, bwy (0,1,0) west, bwz (0,0,1) up
110  // vector space local L:
111  // Basis: M ,H, -A
112  // W(1,0,0) => L(M)
113  // W(0,1,0) => L(-H)
114  // W(0,0,1) => L(A)
115 
116  // R: Transform Matrix local => world equals basis of L, because basis of W is I
117  R[0][0] = Mx; R[0][1] = -Hx; R[0][2] = Ax;
118  R[1][0] = My; R[1][1] = -Hy; R[1][2] = Ay;
119  R[2][0] = Mz; R[2][1] = -Hz; R[2][2] = Az;
120  break;
121 
122  default:
123  case WorldFrame::ENU:
124  // vector space world W:
125  // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up
126  // vector space local L:
127  // Basis: H, M , A
128  // W(1,0,0) => L(H)
129  // W(0,1,0) => L(M)
130  // W(0,0,1) => L(A)
131 
132  // R: Transform Matrix local => world equals basis of L, because basis of W is I
133  R[0][0] = Hx; R[0][1] = Mx; R[0][2] = Ax;
134  R[1][0] = Hy; R[1][1] = My; R[1][2] = Ay;
135  R[2][0] = Hz; R[2][1] = Mz; R[2][2] = Az;
136  break;
137  }
138 
139  // Matrix.getRotation assumes vector rotation, but we're using
140  // coordinate systems. Thus negate rotation angle (inverse).
141  tf2::Quaternion q;
142  R.getRotation(q);
143  tf2::convert(q.inverse(), orientation);
144  return true;
145 }
146 
147 
150  geometry_msgs::Vector3 A,
151  geometry_msgs::Quaternion& orientation) {
152 
153  // This implementation could be optimized regarding speed.
154 
155  // magnetic Field E must not be parallel to A,
156  // choose an arbitrary orthogonal vector
157  geometry_msgs::Vector3 E;
158  if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) {
159  E.x = A.y;
160  E.y = A.x;
161  E.z = 0.0;
162  } else if (fabs(A.z) > 0.1) {
163  E.x = 0.0;
164  E.y = A.z;
165  E.z = A.y;
166  } else {
167  // free fall
168  return false;
169  }
170 
171  return computeOrientation(frame, A, E, orientation);
172 }
Quaternion inverse() const
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
static bool computeOrientation(WorldFrame::WorldFrame frame, geometry_msgs::Vector3 acceleration, geometry_msgs::Vector3 magneticField, geometry_msgs::Quaternion &orientation)
void getRotation(Quaternion &q) const
static void crossProduct(T ax, T ay, T az, T bx, T by, T bz, T &rx, T &ry, T &rz)
void convert(const A &a, B &b)
static T normalizeVector(T &vx, T &vy, T &vz)


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00