1 /*
2 * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.inertial.estimators;
17
18 import com.irurueta.algebra.ArrayUtils;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.algebra.WrongSizeException;
22 import com.irurueta.geometry.Quaternion;
23 import com.irurueta.navigation.frames.CoordinateTransformation;
24 import com.irurueta.navigation.frames.FrameType;
25 import com.irurueta.navigation.frames.NEDPosition;
26 import com.irurueta.navigation.inertial.BodyKinematics;
27
28 /**
29 * This implementation provides slightly more accurate
30 * roll and pitch attitude angles than the ones obtained by
31 * {@link LevelingEstimator}, since north component of gravity in a
32 * local navigation frame is not neglected, because Earth is not
33 * considered to be fully spherical.
34 * <p>
35 * To get this slight improvement of accuracy, this estimator requires
36 * knowledge of device position (latitude and height) on Earth.
37 *
38 * @see LevelingEstimator
39 */
40 public class LevelingEstimator2 {
41
42 /**
43 * Private constructor to prevent instantiation.
44 */
45 private LevelingEstimator2() {
46 }
47
48 /**
49 * Gets body attitude expressed in the local navigation frame.
50 *
51 * @param latitude device latitude expressed in radians (rad).
52 * @param height device height expressed in meters (m).
53 * @param fx x-coordinate of measured body specific force
54 * expressed in meters per squared second (m/s^2).
55 * @param fy y-coordinate of measured body specific force
56 * expressed in meters per squared second (m/s^2).
57 * @param fz z-coordinate of measured body specific force
58 * expressed in meters per squared second (m/s^2).
59 * @param angularRateX x-coordinate of body angular rate expressed in
60 * radians per second (rad/s).
61 * @param angularRateY y-coordinate of body angular rate expressed in
62 * radians per second (rad/s).
63 * @param angularRateZ z-coordinate of body angular rate expressed in
64 * radians per second (rad/s).
65 * @param result instance where attitude will be stored.
66 */
67 public static void getAttitude(
68 final double latitude, final double height,
69 final double fx, final double fy, final double fz,
70 final double angularRateX, final double angularRateY, final double angularRateZ,
71 final CoordinateTransformation result) {
72
73 getPartialAttitude(latitude, height, fx, fy, fz, result);
74
75 // fix yaw angle
76 final var roll = result.getRollEulerAngle();
77 final var pitch = result.getPitchEulerAngle();
78 final var yaw = LevelingEstimator.getYaw(roll, pitch, angularRateX, angularRateY, angularRateZ);
79
80 result.setEulerAngles(roll, pitch, yaw);
81 }
82
83 /**
84 * Gets body attitude expressed in the local navigation frame.
85 *
86 * @param position device position expressed in NED frame.
87 * @param fx x-coordinate of measured body specific force
88 * expressed in meters per squared second (m/s^2).
89 * @param fy y-coordinate of measured body specific force
90 * expressed in meters per squared second (m/s^2).
91 * @param fz z-coordinate of measured body specific force
92 * expressed in meters per squared second (m/s^2).
93 * @param angularRateX x-coordinate of body angular rate expressed in
94 * radians per second (rad/s).
95 * @param angularRateY y-coordinate of body angular rate expressed in
96 * radians per second (rad/s).
97 * @param angularRateZ z-coordinate of body angular rate expressed in
98 * radians per second (rad/s).
99 * @param result instance where attitude will be stored.
100 */
101 public static void getAttitude(
102 final NEDPosition position,
103 final double fx, final double fy, final double fz,
104 final double angularRateX, final double angularRateY, final double angularRateZ,
105 final CoordinateTransformation result) {
106 getAttitude(position.getLatitude(), position.getHeight(), fx, fy, fz, angularRateX, angularRateY, angularRateZ,
107 result);
108 }
109
110 /**
111 * Gets body attitude expressed in the local navigation frame.
112 *
113 * @param latitude device latitude expressed in radians (rad).
114 * @param height device height expressed in meters (m).
115 * @param kinematics body kinematics containing measured
116 * body specific force and angular rate.
117 * @param result instance where attitude will be stored.
118 */
119 public static void getAttitude(
120 final double latitude, final double height, final BodyKinematics kinematics,
121 final CoordinateTransformation result) {
122 getAttitude(latitude, height, kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
123 kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(), result);
124 }
125
126 /**
127 * Gets body attitude expressed in the local navigation frame.
128 *
129 * @param position device position expressed in NED frame.
130 * @param kinematics body kinematics containing measured
131 * body specific force and angular rate.
132 * @param result instance where attitude will be stored.
133 */
134 public static void getAttitude(
135 final NEDPosition position, final BodyKinematics kinematics, final CoordinateTransformation result) {
136 getAttitude(position.getLatitude(), position.getHeight(), kinematics, result);
137 }
138
139 /**
140 * Gets body attitude expressed in the local navigation frame.
141 *
142 * @param latitude device latitude expressed in radians (rad).
143 * @param height device height expressed in meters (m).
144 * @param fx x-coordinate of measured body specific force
145 * expressed in meters per squared second (m/s^2).
146 * @param fy y-coordinate of measured body specific force
147 * expressed in meters per squared second (m/s^2).
148 * @param fz z-coordinate of measured body specific force
149 * expressed in meters per squared second (m/s^2).
150 * @param angularRateX x-coordinate of body angular rate expressed in
151 * radians per second (rad/s).
152 * @param angularRateY y-coordinate of body angular rate expressed in
153 * radians per second (rad/s).
154 * @param angularRateZ z-coordinate of body angular rate expressed in
155 * radians per second (rad/s).
156 * @return estimated attitude.
157 */
158 public static CoordinateTransformation getAttitude(
159 final double latitude, final double height,
160 final double fx, final double fy, final double fz,
161 final double angularRateX, final double angularRateY, final double angularRateZ) {
162 final var result = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
163 getAttitude(latitude, height, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
164 return result;
165 }
166
167 /**
168 * Gets body attitude expressed in the local navigation frame.
169 *
170 * @param position device position expressed in NED frame.
171 * @param fx x-coordinate of measured body specific force
172 * expressed in meters per squared second (m/s^2).
173 * @param fy y-coordinate of measured body specific force
174 * expressed in meters per squared second (m/s^2).
175 * @param fz z-coordinate of measured body specific force
176 * expressed in meters per squared second (m/s^2).
177 * @param angularRateX x-coordinate of body angular rate expressed in
178 * radians per second (rad/s).
179 * @param angularRateY y-coordinate of body angular rate expressed in
180 * radians per second (rad/s).
181 * @param angularRateZ z-coordinate of body angular rate expressed in
182 * radians per second (rad/s).
183 * @return estimated attitude.
184 */
185 public static CoordinateTransformation getAttitude(
186 final NEDPosition position, final double fx, final double fy, final double fz,
187 final double angularRateX, final double angularRateY, final double angularRateZ) {
188 final var result = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
189 getAttitude(position, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
190 return result;
191 }
192
193 /**
194 * Gets body attitude expressed in the local navigation frame.
195 *
196 * @param latitude device latitude expressed in radians (rad).
197 * @param height device height expressed in meters (m).
198 * @param kinematics body kinematics containing measured
199 * body specific force and angular rate.
200 * @return estimated attitude.
201 */
202 public static CoordinateTransformation getAttitude(
203 final double latitude, final double height, final BodyKinematics kinematics) {
204 final var result = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
205 getAttitude(latitude, height, kinematics, result);
206 return result;
207 }
208
209 /**
210 * Gets body attitude expressed in the local navigation frame.
211 *
212 * @param position device position expressed in NED frame.
213 * @param kinematics body kinematics containing measured
214 * body specific force and angular rate.
215 * @return estimated attitude.
216 */
217 public static CoordinateTransformation getAttitude(final NEDPosition position, final BodyKinematics kinematics) {
218 final var result = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
219 getAttitude(position, kinematics, result);
220 return result;
221 }
222
223 /**
224 * Gets partial body attitude where only roll and pitch angles
225 * are reliable.
226 *
227 * @param latitude device latitude expressed in radians (rad).
228 * @param height device height expressed in meters (m).
229 * @param fx x-coordinate of measured body specific force
230 * expressed in meters per squared second (m/s^2).
231 * @param fy y-coordinate of measured body specific force
232 * expressed in meters per squared second (m/s^2).
233 * @param fz z-coordinate of measured body specific force
234 * expressed in meters per squared second (m/s^2).
235 * @param result instance where partial body attitude will be stored.
236 */
237 static void getPartialAttitude(
238 final double latitude, final double height, final double fx, final double fy, final double fz,
239 final CoordinateTransformation result) {
240
241 try {
242 // get normalized vector from measured specific force, which
243 // mainly contains sensed gravity in the local navigation frame
244 // when device is static (Coriolis force is neglected in this
245 // implementation).
246
247 // obtain normalized specific force in local navigation coordinates
248 final var normF = new double[]{fx, fy, fz};
249 ArrayUtils.normalize(normF);
250
251 // obtain gravity in NED coordinates (locally equivalent to
252 // the one in local navigation frame).
253 // Because Earth is not fully spherical, normalized vector won't
254 // be (0, 0, 1), because there will always be a small north
255 // gravity component.
256 final var nedGravity = NEDGravityEstimator.estimateGravityAndReturnNew(latitude, height);
257
258 final var normG = nedGravity.asArray();
259
260 // ensure that down coordinate points towards Earth center, just
261 // like sensed specific force
262 ArrayUtils.multiplyByScalar(normG, -1.0, normG);
263
264 ArrayUtils.normalize(normG);
265
266 // compute angle between both normalized vectors using dot product
267 // cos(alpha) = normF' * normG
268 final var cosAlpha = ArrayUtils.dotProduct(normF, normG);
269
270 // compute vector perpendicular to both normF and normG which will
271 // be the rotation axis
272 final var skew = Utils.skewMatrix(normG);
273 final var tmp1 = Matrix.newFromArray(normF);
274 final var tmp2 = skew.multiplyAndReturnNew(tmp1);
275
276 final var sinAlpha = Utils.normF(tmp2);
277
278 final var axis = tmp2.toArray();
279 ArrayUtils.normalize(axis);
280
281 final var alpha = Math.atan2(sinAlpha, cosAlpha);
282
283 final var q = new Quaternion(axis, alpha);
284
285 // set rotation (yaw angle will be arbitrary and will need
286 // to be fixed later on)
287 result.setSourceType(FrameType.LOCAL_NAVIGATION_FRAME);
288 result.setDestinationType(FrameType.BODY_FRAME);
289 result.fromRotation(q);
290
291 } catch (final WrongSizeException ignore) {
292 // never happens
293 }
294 }
295 }