pose.py
Go to the documentation of this file.
1 '''
2 Created 9/4/2013
3 
4 Author: Walt Johnson
5 '''
6 from __future__ import print_function
7 
8 from numpy import sin, cos, arccos, arcsin, arctan2
9 from numpy import r_, c_
10 from numpy import dot
11 from numpy import pi
12 import numpy as np
13 from tqdm import tqdm
14 
15 # import pylib.plotTools as pt
16 
17 
18 def quatInit():
19  q = np.zeros(4)
20  q[0] = 1.0
21  return q
22 
23 # Quaternion Conjugate: q* = [ w, -x, -y, -z ] of quaterion q = [ w, x, y, z ]
24 # Rotation in opposite direction.
25 def quatConj( q ):
26  return r_[ q[0], -q[1], -q[2], -q[3] ]
27 
28 # Quaternion Inverse: q^-1 = q* / |q|^2
29 def quatInv( q ):
30  return quatConj(q)/np.square(np.linalg.norm(q))
31 
32 
33 # * Product of two Quaternions. Order of q1 and q2 matters (same as applying two successive DCMs)!!!
34 # * Concatenates two quaternion rotations into one.
35 # * result = q1 * q2
36 # * References:
37 # * http://www.mathworks.com/help/aeroblks/quaternionmultiplication.html
38 # * http://physicsforgames.blogspot.com/2010/02/quaternions.html
39 def mul_Quat_Quat( q1, q2 ):
40  result = np.zeros(4)
41 
42  result[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
43  result[1] = q1[0]*q2[1] + q1[1]*q2[0] - q1[2]*q2[3] + q1[3]*q2[2];
44  result[2] = q1[0]*q2[2] + q1[1]*q2[3] + q1[2]*q2[0] - q1[3]*q2[1];
45  result[3] = q1[0]*q2[3] - q1[1]*q2[2] + q1[2]*q2[1] + q1[3]*q2[0];
46  return result
47 
48 
49 # * Division of two Quaternions. Order matters!!!
50 # * result = q1 / q2.
51 # * Reference: http://www.mathworks.com/help/aeroblks/quaterniondivision.html
52 def div_Quat_Quat( q1, q2 ):
53  result = np.zeros(4)
54  d = 1.0 / (q1[0]*q1[0] + q1[1]*q1[1] + q1[2]*q1[2] + q1[3]*q1[3])
55 
56  result[0] = (q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3]) * d
57  result[1] = (q1[0]*q2[1] - q1[1]*q2[0] - q1[2]*q2[3] + q1[3]*q2[2]) * d
58  result[2] = (q1[0]*q2[2] + q1[1]*q2[3] - q1[2]*q2[0] - q1[3]*q2[1]) * d
59  result[3] = (q1[0]*q2[3] - q1[1]*q2[2] + q1[2]*q2[1] - q1[3]*q2[0]) * d
60  return result
61 
62 
63 # Quaternion rotation from vector v1 to vector v2.
64 # Reference:
65 def quat_Vec3_Vec3( v1, v2 ):
66 # Vector3_t w1, w2;
67 
68  # Normalize input vectors
69  w1 = v1 / np.linalg.norm(v1)
70  w2 = v2 / np.linalg.norm(v2)
71 
72  qResult = np.zeros(4)
73  # q[1:3]
74  qResult[1:4] = np.cross(w1, w2)
75 
76  # q[0]
77  qResult[0] = np.sqrt( np.square( np.dot(w1,w1) ) ) + np.dot(w1, w2)
78 
79  # Normalize quaternion
80  qResult = qResult / np.linalg.norm(qResult)
81  return qResult
82 
83 
84 def normalize( v ):
85  return v / np.linalg.norm(v)
86 
87 
88 # Computationally simple means to apply quaternion rotation to a vector
89 # Requires quaternion be normalized first
90 def quatRotVect( q, v ):
91  t = 2.0 * np.cross(q[1:4], v)
92  result = v + q[0] * t + np.cross(q[1:4], t)
93  return result
94 
95 def quatRotVectArray( q, v ):
96  t = 2.0 * np.cross(q[:,1:4], v)
97  result = v + (q[:,0] * t.T).T + np.cross(q[:,1:4], t)
98  return result
99 
100 
101 # Find quaternion interpolation between two quaterions. Blend must be 0 to 1.
102 # Reference: http://physicsforgames.blogspot.com/2010/02/quaternions.html
103 def quatNLerp( q1, q2, blend):
104  result = np.zeros(4)
105 
106  dot = q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3]
107  blendI = 1.0 - blend
108 
109  if dot < 0.0:
110  tmpF = np.zeros(4)
111  tmpF = -q2
112  result = blendI*q1 + blend*tmpF
113  else:
114  result = blendI*q1 + blend*q2
115 
116  # Normalize quaternion
117  result = result / np.linalg.norm(result)
118 
119  return result
120 
121 
122 # * This will convert from quaternions to euler angles. Ensure quaternion is previously normalized.
123 # * q(4,1) -> euler[phi;theta;psi] (rad)
124 # *
125 # * Reference: http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
126 def quat2euler( q ):
127  theta = np.zeros(3)
128 
129  theta[0] = np.arctan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2]) )
130  theta[1] = np.arcsin( 2 * (q[0]*q[2] - q[3]*q[1]) )
131  theta[2] = np.arctan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3]) )
132  return theta
133 
135  theta = np.empty((np.shape(q)[0],3))
136 
137  theta[:,0] = np.arctan2( 2 * (q[:,0]*q[:,1] + q[:,2]*q[:,3]), 1 - 2 * (q[:,1]*q[:,1] + q[:,2]*q[:,2]) )
138  theta[:,1] = np.arcsin( 2 * (q[:,0]*q[:,2] - q[:,3]*q[:,1]) )
139  theta[:,2] = np.arctan2( 2 * (q[:,0]*q[:,3] + q[:,1]*q[:,2]), 1 - 2 * (q[:,2]*q[:,2] + q[:,3]*q[:,3]) )
140  return theta
141 
142 
143 
144 # * This will convert from euler angles to quaternion vector
145 # * phi, theta, psi -> q(4,1)
146 # * euler angles in radians
147 def euler2quat( euler ):
148  q = np.zeros(4)
149  hphi = euler[0] * 0.5
150  hthe = euler[1] * 0.5
151  hpsi = euler[2] * 0.5
152 
153  shphi = np.sin(hphi)
154  chphi = np.cos(hphi)
155 
156  shthe = np.sin(hthe)
157  chthe = np.cos(hthe)
158 
159  shpsi = np.sin(hpsi)
160  chpsi = np.cos(hpsi)
161 
162  q[0] = chphi * chthe * chpsi + shphi * shthe * shpsi
163  q[1] = shphi * chthe * chpsi - chphi * shthe * shpsi
164  q[2] = chphi * shthe * chpsi + shphi * chthe * shpsi
165  q[3] = chphi * chthe * shpsi - shphi * shthe * chpsi
166  return q
167 
168 def euler2quatArray( euler ):
169  q = np.zeros((np.shape(euler)[0],4))
170  hphi = euler[:,0] * 0.5
171  hthe = euler[:,1] * 0.5
172  hpsi = euler[:,2] * 0.5
173 
174  shphi = np.sin(hphi)
175  chphi = np.cos(hphi)
176 
177  shthe = np.sin(hthe)
178  chthe = np.cos(hthe)
179 
180  shpsi = np.sin(hpsi)
181  chpsi = np.cos(hpsi)
182 
183  q[:,0] = chphi * chthe * chpsi + shphi * shthe * shpsi
184  q[:,1] = shphi * chthe * chpsi - chphi * shthe * shpsi
185  q[:,2] = chphi * shthe * chpsi + shphi * chthe * shpsi
186  q[:,3] = chphi * chthe * shpsi - shphi * shthe * chpsi
187  return q
188 
189 # NE to heading/body frame
190 def psiDCM(psi):
191  cpsi = cos(psi) # cos(psi)
192  spsi = sin(psi) # sin(psi)
193 
194  DCM = r_[
195  c_[ cpsi, spsi ],
196  c_[ -spsi, cpsi ],
197  ]
198 
199  return DCM
200 
201 
202 def DCMpsi(A):
203  psi = arctan2( A[0,1], A[0,0] )
204 
205  return psi
206 
207 
208 # NED to body frame - In the 1-2-3 (roll, pitch, yaw) order
209 def eulerDCM(euler):
210  cphi = cos(euler[0]) # cos(phi)
211  cthe = cos(euler[1]) # cos(theta)
212  cpsi = cos(euler[2]) # cos(psi)
213 
214  sphi = sin(euler[0]) # sin(phi)
215  sthe = sin(euler[1]) # sin(theta)
216  spsi = sin(euler[2]) # sin(psi)
217 
218  DCM = r_[
219  c_[ cthe*cpsi, cthe*spsi, -sthe ],
220  c_[ -cphi*spsi + sphi*sthe*cpsi, cphi*cpsi + sphi*sthe*spsi, sphi*cthe ],
221  c_[ sphi*spsi + cphi*sthe*cpsi, -sphi*cpsi + cphi*sthe*spsi, cphi*cthe ],
222  ]
223 
224  return DCM
225 
226 
227 def DCMeuler(A):
228  phi = arctan2( A[1,2], A[2,2] )
229  the = arcsin( -A[0,2] )
230  psi = arctan2( A[0,1], A[0,0] )
231 
232  return r_[ phi, the, psi ]
233 
234 
235 def DCMeulerToPsi(A, phi, the):
236  psi = arctan2( A[0,1]/cos(the), A[0,0]/cos(the) )
237 
238  return r_[ phi, the, psi ]
239 
240 
241 # /*
242 # * This will construct a direction cosine matrix from
243 # * quaternions in the standard rotation sequence
244 # * [phi][theta][psi] from NED to body frame
245 # *
246 # * body = tBL(3,3)*NED
247 # * q(4,1)
248 # *
249 # * Reference: http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29
250 # */
251 def quatDCM(q):
252  DCM = r_[
253  c_[ 1.0 - 2 * (q[2]*q[2] + q[3]*q[3]), 2 * (q[1]*q[2] + q[0]*q[3]), 2 * (q[1]*q[3] - q[0]*q[2]) ],
254  c_[ 2 * (q[1]*q[2] - q[0]*q[3]), 1.0 - 2 * (q[1]*q[1] + q[3]*q[3]), 2 * (q[2]*q[3] + q[0]*q[1]) ],
255  c_[ 2 * (q[1]*q[3] + q[0]*q[2]), 2 * (q[2]*q[3] - q[0]*q[1]), 1.0 - 2 * (q[1]*q[1] + q[2]*q[2]) ],
256  ]
257 
258  return DCM
259 
260 # * This will construct quaternions from a direction cosine
261 # * matrix in the standard rotation sequence.
262 # * [phi][theta][psi] from NED to body frame
263 # *
264 # * body = tBL(3,3)*NED
265 # * q(4,1)
266 # *
267 # * Reference: http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29
268 def DCMquat(mat):
269  q = np.zeros(4)
270 
271  q[0] = 0.5 * np.sqrt(1.0 + mat[0,0] + mat[1,1] + mat[2,2])
272 
273  d = 1.0 / (4.0 * q[0])
274 
275  q[1] = d * (mat[1,2] - mat[2,1])
276  q[2] = d * (mat[2,0] - mat[0,2])
277  q[3] = d * (mat[0,1] - mat[1,0])
278 
279  return q
280 
281 
282 # * This will construct the euler omega-cross matrix
283 # * wx(3,3)
284 # * p, q, r (rad/sec)
285 def eulerWx( euler ):
286  mat = np.zeros((3,3))
287 
288  p = euler[0]
289  q = euler[1]
290  r = euler[2]
291 
292  # Row 1
293  mat[0,0] = 0
294  mat[0,1] = -r
295  mat[0,2] = q
296  # Row 2
297  mat[1,0] = r
298  mat[1,1] = 0
299  mat[1,2] = -p
300  # Row 3
301  mat[2,0] = -q
302  mat[2,1] = p
303  mat[2,2] = 0
304 
305  return mat
306 
307 
308 # * This will construct the quaternion omega matrix
309 # * W(4,4)
310 # * p, q, r (rad/sec)
311 def quatW( euler ):
312 
313  mat = np.zeros((4,4))
314  p = euler[0] * 0.5
315  q = euler[1] * 0.5
316  r = euler[2] * 0.5
317 
318  # Row 1
319  mat[0,0] = 0
320  mat[0,1] = -p
321  mat[0,2] = -q
322  mat[0,3] = -r
323  # Row 2
324  mat[1,0] = p
325  mat[1,1] = 0
326  mat[1,2] = r
327  mat[1,3] = -q
328  # Row 3
329  mat[2,0] = q
330  mat[2,1] = -r
331  mat[2,2] = 0
332  mat[2,3] = p
333  # Row 4
334  mat[3,0] = r
335  mat[3,1] = q
336  mat[3,2] = -p
337  mat[3,3] = 0
338 
339  return mat
340 
341 
342 # * Convert quaternion to rotation axis (and angle). Quaternion must be normalized.
343 def quatRotAxis( q ):
344 
345  # Normalize quaternion
346  q = q / np.linalg.norm(q)
347 
348  theta = np.arccos( q[0] ) * 2.0
349  sin_a = np.sqrt( 1.0 - q[0] * q[0] )
350 
351  if np.fabs( sin_a ) < 0.0005:
352  sin_a = 1.0
353 
354  d = 1.0 / sin_a;
355 
356  pqr = np.zeros(3)
357  pqr[0] = q[1] * d;
358  pqr[1] = q[2] * d;
359  pqr[2] = q[3] * d;
360 
361  print(theta * 180/pi)
362 
363  return pqr
364 
365 
366 # * Compute the derivative of the Euler_t angle psi with respect
367 # * to the quaternion Q. The result is a row vector
368 # *
369 # * d(psi)/d(q0)
370 # * d(psi)/d(q1)
371 # * d(psi)/d(q2)
372 # * d(psi)/d(q3)
373 def dpsi_dq( q ):
374  dq = np.zeros(4)
375 
376  t1 = 1 - 2 * (q[2]*q[2] + q[3]*q[2])
377  t2 = 2 * (q[1]*q[2] + q[0]*q[3])
378  err = 2 / ( t1*t1 + t2*t2 )
379 
380  dq[0] = err * (q[3]*t1)
381  dq[1] = err * (q[2]*t1)
382  dq[2] = err * (q[1]*t1 + 2 * q[2]*t2)
383  dq[3] = err * (q[0]*t1 + 2 * q[3]*t2)
384 
385  return dq
386 
387 
388 # Find Earth Centered Earth Fixed coordinate from LLA
389 #
390 # lla[0] = latitude (decimal degree)
391 # lla[1] = longitude (decimal degree)
392 # lla[2] = msl altitude (m)
393 def lla2ecef( lla, metric=True):
394  e = 0.08181919084262 # Earth first eccentricity: e = sqrt((R^2-b^2)/R^2);
395  # double R, b, Rn, Smu, Cmu, Sl, Cl;
396 
397  # Earth equatorial and polar radii (from flattening, f = 1/298.257223563;
398  if metric:
399  R = 6378137 # m
400  # Earth polar radius b = R * (1-f)
401  b = 6356752.31424518
402  else:
403  R = 20925646.3255 # ft
404  # Earth polar radius b = R * (1-f)
405  b = 20855486.5953
406 
407  LLA = lla * pi/180;
408 
409  Smu = sin(LLA[:,0]);
410  Cmu = cos(LLA[:,0]);
411  Sl = sin(LLA[:,1]);
412  Cl = cos(LLA[:,1]);
413 
414  # Radius of curvature at a surface point:
415  Rn = R / np.sqrt(1 - e*e*Smu*Smu)
416 
417  Pe = np.zeros(np.shape(LLA))
418  Pe[:,0] = (Rn + LLA[:,2]) * Cmu * Cl
419  Pe[:,1] = (Rn + LLA[:,2]) * Cmu * Sl
420  Pe[:,2] = (Rn*b*b/R/R + LLA[:,2]) * Smu
421 
422  return Pe
423 
424 # Find NED (north, east, down) from LLAref to LLA
425 #
426 # lla[0] = latitude (decimal degree)
427 # lla[1] = longitude (decimal degree)
428 # lla[2] = msl altitude (m)
429 def lla2ned( llaRef, lla ):
430  a2d = 111120.0 # = DEG2RAD * earth_radius_in_meters
431 
432  deltaLLA = np.zeros(np.shape(lla))
433  deltaLLA[:,0] = lla[:,0] - llaRef[0]
434  deltaLLA[:,1] = lla[:,1] - llaRef[1]
435  deltaLLA[:,2] = lla[:,2] - llaRef[2]
436 
437  ned = np.zeros(np.shape(lla))
438  ned[:,0] = deltaLLA[:,0] * a2d
439  ned[:,1] = deltaLLA[:,1] * a2d * cos(llaRef[0] * pi/180)
440  ned[:,2] = -deltaLLA[:,2]
441 
442  return ned
443 
444 
445 # Find NED (north, east, down) from LLAref to LLA
446 #
447 # lla[0] = latitude (decimal degree)
448 # lla[1] = longitude (decimal degree)
449 # lla[2] = msl altitude (m)
450 def lla2ned_single( llaRef, lla ):
451  a2d = 111120.0 # = DEG2RAD * earth_radius_in_meters
452 
453  deltaLLA = np.zeros(np.shape(lla))
454  deltaLLA[0] = lla[0] - llaRef[0]
455  deltaLLA[1] = lla[1] - llaRef[1]
456  deltaLLA[2] = lla[2] - llaRef[2]
457 
458  ned = np.zeros(np.shape(lla))
459  ned[0] = deltaLLA[0] * a2d
460  ned[1] = deltaLLA[1] * a2d * cos(llaRef[0] * pi/180)
461  ned[2] = -deltaLLA[2]
462 
463  return ned
464 
465 
466 # Find Delta LLA of NED (north, east, down) from LLAref
467 #
468 # lla[0] = latitude (decimal degree)
469 # lla[1] = longitude (decimal degree)
470 # lla[2] = msl altitude (m)
471 def ned2DeltaLla( ned, llaRef ):
472  invA2d = 1.0 / 111120.0 # 1 / radius
473 
474  deltaLLA = np.zeros(np.shape(ned))
475  deltaLLA[:,0] = ned[:,0] * invA2d
476  deltaLLA[:,1] = ned[:,1] * invA2d / cos(llaRef[0] * pi/180)
477  deltaLLA[:,2] = -ned[:,2]
478 
479  return deltaLLA
480 
481 # Find LLA of NED (north, east, down) from LLAref
482 #
483 # lla[0] = latitude (decimal degree)
484 # lla[1] = longitude (decimal degree)
485 # lla[2] = msl altitude (m)
486 def ned2lla( ned, llaRef ):
487  deltaLLA = ned2DeltaLla( ned, llaRef )
488 
489  lla = np.zeros(np.shape(ned))
490  lla[:,0] = llaRef[:,0] + deltaLLA[:,0]
491  lla[:,1] = llaRef[:,1] + deltaLLA[:,1]
492  lla[:,2] = llaRef[:,2] + deltaLLA[:,2]
493 
494  return lla
495 
496 def rotate_ned2ecef( latlon ):
497  R = np.zeros((3,3))
498  # Smu, Cmu, Sl, Cl
499 
500  Smu = sin(latlon[0])
501  Cmu = cos(latlon[0])
502  Sl = sin(latlon[1])
503  Cl = cos(latlon[1])
504 
505  R[0,0] = -Smu * Cl
506  R[0,1] = -Sl
507  R[0,2] = -Cmu * Cl
508  R[1,0] = -Smu * Sl
509  R[1,1] = Cl
510  R[1,2] = -Cmu * Sl
511  R[2,0] = Cmu
512  R[2,1] = 0.0
513  R[2,2] = -Smu
514  return R
515 
516 def rotate_ecef2ned( latlon ):
517  R = np.zeros((3,3))
518  # Smu, Cmu, Sl, Cl
519 
520  Smu = sin(latlon[0])
521  Cmu = cos(latlon[0])
522  Sl = sin(latlon[1])
523  Cl = cos(latlon[1])
524 
525  R[0,0] = -Smu * Cl
526  R[0,1] = -Smu * Sl
527  R[0,2] = Cmu
528 
529  R[1,0] = -Sl
530  R[1,1] = Cl
531  R[1,2] = 0.0
532 
533  R[2,0] = -Cmu * Cl
534  R[2,1] = -Cmu * Sl
535  R[2,2] = -Smu
536  return R
537 
538 
539 # * NED to Euler_t
540 def nedEuler(ned):
541  euler = np.zeros(np.shape(ned))
542  euler[:,2] = arctan2( ned[:,1], ned[:,0] )
543  euler[:,1] = arctan2( -ned[:,2], np.sqrt( ned[:,0]**2 + ned[:,1]**2 ) )
544  euler[:,0] = 0
545  return euler
546 
547 
548 # * Euler_t to NED
549 def eulerNed( e ):
550  ned = np.zeros(3)
551  v = r_[ 1., 0., 0.]
552  vectorRotateBodyToInertial( v, e, ned )
553  return ned
554 
555 
556 # Rotate theta eulers from body to inertial frame by ins eulers, in order: phi, theta, psi
558  eResult = np.zeros(np.shape(e))
559 
560  Ai = eulerDCM(rot) # use estimate
561 
562  for i in range(0,np.shape(eResult)[0]):
563  # Create DCMs (rotation matrices)
564  At = eulerDCM(e[i,:])
565 
566  # Apply INS Rotation to Desired Target vector
567  AiAt = dot(Ai, At) # Apply rotation
568  eResult[i,:] = DCMeuler(AiAt) # Pull out new eulers
569 
570  return eResult
571 
572 # Rotate theta eulers from inertial to body frame by ins eulers, in order: psi, theta, phi
574  eResult = np.zeros(np.shape(e))
575 
576  Ai = eulerDCM(rot) # use estimate
577 
578  for i in range(0,np.shape(eResult)[0]):
579  # Create DCMs (rotation matrices)
580  At = eulerDCM(e[i,:])
581 
582  # Apply INS Rotation to Desired Target vector
583  AiAt = dot(Ai.T, At) # Apply rotation
584  eResult[i,:] = DCMeuler(AiAt) # Pull out new eulers
585 
586  return eResult
587 
588 # Rotate vector from inertial to body frame by euler angles, in order: psi, theta, phi
589 def vectorRotateInertialToBody( vIn, eRot ):
590  vResult = np.zeros(np.shape(vIn))
591 
592  for i in range(0,np.shape(vResult)[0]):
593  # Create DCM (rotation matrix)
594  DCM = eulerDCM(eRot[i,:])
595  # Apply rotation to vector
596  vResult[i,:] = np.dot( DCM, vIn[i,:] )
597 
598  return vResult
599 
600 # Rotate vector from body to inertial frame by euler angles, in order: phi, theta, psi
601 def vectorRotateBodyToInertial( vIn, eRot ):
602  vResult = np.zeros(np.shape(vIn))
603 
604  for i in range(0,np.shape(vResult)[0]):
605  # Create DCM (rotation matrix)
606  DCM = eulerDCM(eRot[i,:])
607  # Apply rotation to vector
608  vResult[i,:] = np.dot( DCM.T, vIn[i,:] )
609 
610  return vResult
611 
612 # Rotate vector from inertial to body frame by euler angles, in order: psi, theta, phi
614  vResult = np.zeros(np.shape(vIn))
615 
616  # Create DCM (rotation matrix)
617  DCM = eulerDCM(eRot)
618 
619  for i in range(0,np.shape(vResult)[0]):
620  # Apply rotation to vector
621  vResult[i,:] = np.dot( DCM, vIn[i,:] )
622 
623  return vResult
624 
625 # Rotate vector from body to inertial frame by euler angles, in order: phi, theta, psi
627  vResult = np.zeros(np.shape(vIn))
628 
629  # Create DCM (rotation matrix)
630  DCM = eulerDCM(eRot)
631 
632  for i in range(0,np.shape(vResult)[0]):
633  # Apply rotation to vector
634  vResult[i,:] = np.dot( DCM.T, vIn[i,:] )
635 
636  return vResult
637 
638 
639 # Find euler angles of a vector (no roll)
640 def vectorEuler( v ):
641  psi = np.arctan2( v[1], v[0] )
642  dcm = psiDCM( psi )
643  v2 = np.dot( dcm, v[0:2] )
644  theta = np.arctan2( -v[2], v2[0] )
645  result = r_[ 0., theta, psi ]
646 # print("v:", v, " v2:", v2, " e:", result * 180/pi)
647  return result
648 
649 
650 def vectorQuat( v ):
651  return euler2quat( vectorEuler(v) )
652 
653 
654 def unwrapAngle(angle):
655 
656  twoPi = pi*2
657 
658  for i in range(0,np.shape(angle)[0]):
659  while angle[i] < -pi:
660  angle[i] += twoPi
661  while angle[i] > pi:
662  angle[i] -= twoPi
663 
664  return angle
665 
666 # Non-accelerated gravity used to determine attitude
667 def accellToEuler(acc):
668  euler = np.zeros(np.shape(acc))
669 
670  euler[:,0] = np.arctan2( -acc[:,1], -acc[:,2] )
671  euler[:,1] = np.arctan2( acc[:,0], np.sqrt( acc[:,1]*acc[:,1] + acc[:,2]*acc[:,2]) );
672  return euler;
673 
674 def acc2AttAndBias(acc):
675  att = np.zeros(np.shape(acc))
676  bias = np.zeros(np.shape(acc))
677 
678 # pe = pt.cPlot()
679 
680  for i in range(0,4):
681  att = accellToEuler(acc-bias)
682 
683  gIF = np.r_[ 0, 0, -9.80665 ]
684  for i in range(0,np.shape(bias)[0]):
685  # Create DCM (rotation matrix)
686  DCM = eulerDCM(att[i,:])
687  # Apply rotation to vector: inertial -> body
688  g = np.dot( DCM, gIF )
689  bias[i,:] = acc[i,:] - g
690 
691 # pe.plot3Axes(1, range(0,np.shape(bias)[0]), bias, 'Bias', 'm/s^2')
692 
693  return [att, bias]
694 
695 def norm(v, axis=None):
696  return np.sqrt(np.sum(v*v, axis=axis))
697 
698 qmat_matrix = np.array([[[1.0, 0, 0, 0],
699  [0, -1.0, 0, 0],
700  [0, 0, -1.0, 0],
701  [0, 0, 0, -1.0]],
702  [[0, 1.0, 0, 0],
703  [1.0, 0, 0, 0],
704  [0, 0, 0, 1.0],
705  [0, 0, -1.0, 0]],
706  [[0, 0, 1.0, 0],
707  [0, 0, 0, -1.0],
708  [1.0, 0, 0, 0],
709  [0, 1.0, 0, 0]],
710  [[0, 0, 0, 1.0],
711  [0, 0, 1.0, 0],
712  [0, -1.0, 0, 0],
713  [1.0, 0, 0, 0]]])
714 
715 def qmult(q1, q2):
716  if q1.shape[0] == 1 and q2.shape[0] == 1:
717  dots = np.empty_like(q2)
718  for i in range(q2.shape[0]):
719  dots[i, :] = qmat_matrix.dot(q2.T).squeeze().dot(q1.T).T
720  elif q1.shape[0] == 1 and q2.shape[0] != 1:
721  dots = np.empty_like(q2)
722  for i in range(q2.shape[0]):
723  dots[i, :] = qmat_matrix.dot(q2[i, :].T).squeeze().dot(q1.T).T
724  elif q1.shape[0] != 1 and q2.shape[0] == 1:
725  dots = np.empty_like(q2)
726  for i in range(q2.shape[0]):
727  dots[i, :] = qmat_matrix.dot(q2.T).squeeze().dot(q1[i,:].T).T
728  elif q1.shape[0] == q2.shape[0]:
729  dots = np.empty_like(q2)
730  for i in range(q2.shape[0]):
731  dots[i, :] = qmat_matrix.dot(q2[i,:].T).squeeze().dot(q1[i, :].T).T
732  else:
733  raise Exception("Incompatible quaternion arrays -- cannot multiply")
734 
735  # print qmat_matrix.dot(q2.T).squeeze()
736  # print np.tensordot(qmat_matrix.T, q2, axes=[0,1]).T.squeeze()
737  # dots = np.empty((2,4,4))
738  # for i in range(q1.shape[0]):
739  # dots[i,:] = qmat_matrix.dot(q2[i,:].T)
740  # tensordots = np.tensordot(qmat_matrix.T, q2, axes=[0,1]).T
741  # dots = np.empty_like(q1)
742  # for i in range(q1.shape[0]):
743  # dots[i,:] = qmat_matrix.dot(q2[i,:].T).squeeze().dot(q1[i,:].T).T
744  # tensordots = np.tensordot(q1, np.tensordot(qmat_matrix.T, q2, axes=[0,1]), axes=1).T
745  # print "dots = ", dots, "\ntensordots = ", tensordots
746  # print "diff = ", dots - tensordots
747 
748  # return np.tensordot(q1, np.tensordot(q2.T, qmat_matrix.T, axes=[0,1]).T, axes=1)[0].T
749  return dots
750 
751 def qlog(q):
752  q *= np.sign(q[:,0])[:,None]
753  norm_v = norm(q[:,1:], axis=1)
754  out = np.empty((len(q), 3))
755  idx = norm_v > 1e-4
756  out[~idx] = 2.0 * np.sign(q[~idx, 0, None]) * q[~idx,1:]
757  out[idx] = (2.0 * np.arctan2(norm_v[idx,None], q[idx,0,None])) * q[idx,1:]/norm_v[idx,None]
758  return out
759 
760 def qexp(v):
761  out = np.empty((len(v), 4))
762  norm_v = norm(v, axis=1)
763  idx = norm_v > 1e-4
764  out[~idx, 0] = 1
765  out[~idx, 1:] = v[~idx,:]
766  out[idx] = np.cos(norm_v[idx,None]/2.0)
767  out[idx, 1:] = np.sin(norm_v[idx,None]/2.0) * v[idx,:]/norm_v[idx,None]
768  return out
769 
770 def qinv(q):
771  qcopy = q.copy()
772  qcopy[:,1:] *= -1.0
773  return qcopy
774 
775 def qboxplus(q, v):
776  return qmult(q, qexp(v))
777 
778 def qboxminus(q1, q2):
779  return qlog(qmult(qinv(q2), q1))
780 
781 # Implementation of "Mean of Sigma Points" from Integrating Generic Sensor Fusion Algorithms with
782 # Sound State Representations through Encapsulation of Manifolds by Hertzberg et. al.
783 # https://arxiv.org/pdf/1107.1119.pdf p.13
784 def meanOfQuat(q):
785  n = float(q.shape[0])
786  mu = q[None,0,:]
787  prev_mu = None
788  iter = 0
789  while prev_mu is None or norm(qboxminus(mu, prev_mu)) > 1e-3:
790  iter +=1
791  prev_mu = mu
792  mu = qboxplus(mu, np.sum(qboxminus(q, mu), axis=0)[None,:]/n)
793  assert np.abs(1.0 - norm(mu)) <= 1e03
794  return mu
795 
797  assert q.shape[2] == 4
798  mu = np.empty((q.shape[0], 4))
799  for i in tqdm(range(q.shape[0])):
800  mu[None,i,:] = meanOfQuat(q[i,:,:])
801  return mu
802 
803 
804 # def salemUtLla():
805 # return np.r_[ 40.0557114, -111.6585476, 1426.77 ] # // (deg,deg,m) Lat,Lon,Height above sea level (not HAE, height above ellipsoid)
806 #
807 # def salemUtMagDecInc():
808 # return np.r_[ 0.20303997, 1.141736219 ] # (rad) Declination: 11.6333333 deg or 11 deg 38', Inclination: -65.4166667 deg or 65 deg 25'
809 
810 if __name__ == '__main__':
811  q = np.random.random((5000, 4))
812  q = q / norm(q, axis=1)[:,None]
813  # Find the mean Quaternion
814 
815  q1 = q[0:2, :]
816  q2 = q[2:4, :]
817 
818  q3 = qmult(q1, q2)
819 
820  print("q2 =", q2)
821  print("math =", qmult(qinv(q1), q3))
822  assert np.sqrt(np.sum(np.square(qmult(qinv(q1), q3) - q2))) < 1e-8
823 
824  mu = meanOfQuat(q)
825 
826  qarr = np.random.random((5000, 25, 4))
827  qarr = qarr * 1.0/norm(qarr, axis=2)[:,:,None]
828  mu = meanOfQuatArray(qarr)
829  print(mu)
830 
831  pass
def vectorRotateInertialToBody2(vIn, eRot)
Definition: pose.py:613
def eulerRotateBodyToInertial2(e, rot)
Definition: pose.py:557
def quatRotVectArray(q, v)
Definition: pose.py:95
def qmult(q1, q2)
Definition: pose.py:715
def psiDCM(psi)
Definition: pose.py:190
def mul_Quat_Quat(q1, q2)
Definition: pose.py:39
def lla2ned(llaRef, lla)
Definition: pose.py:429
def quatNLerp(q1, q2, blend)
Definition: pose.py:103
def meanOfQuatArray(q)
Definition: pose.py:796
def lla2ecef(lla, metric=True)
Definition: pose.py:393
def normalize(v)
Definition: pose.py:84
def eulerNed(e)
Definition: pose.py:549
def accellToEuler(acc)
Definition: pose.py:667
def unwrapAngle(angle)
Definition: pose.py:654
GeneratorWrapper< T > range(T const &start, T const &end, T const &step)
Definition: catch.hpp:4141
def qboxminus(q1, q2)
Definition: pose.py:778
def vectorQuat(v)
Definition: pose.py:650
def quat_Vec3_Vec3(v1, v2)
Definition: pose.py:65
def rotate_ned2ecef(latlon)
Definition: pose.py:496
def quatRotVect(q, v)
Definition: pose.py:90
def eulerRotateInertialToBody2(e, rot)
Definition: pose.py:573
def eulerDCM(euler)
Definition: pose.py:209
def lla2ned_single(llaRef, lla)
Definition: pose.py:450
def DCMeuler(A)
Definition: pose.py:227
def ned2DeltaLla(ned, llaRef)
Definition: pose.py:471
def div_Quat_Quat(q1, q2)
Definition: pose.py:52
def norm(v, axis=None)
Definition: pose.py:695
def rotate_ecef2ned(latlon)
Definition: pose.py:516
def quatRotAxis(q)
Definition: pose.py:343
def nedEuler(ned)
Definition: pose.py:540
def eulerWx(euler)
Definition: pose.py:285
def dpsi_dq(q)
Definition: pose.py:373
def qlog(q)
Definition: pose.py:751
def quat2euler(q)
Definition: pose.py:126
def qboxplus(q, v)
Definition: pose.py:775
def ned2lla(ned, llaRef)
Definition: pose.py:486
def quatW(euler)
Definition: pose.py:311
def vectorRotateBodyToInertial2(vIn, eRot)
Definition: pose.py:626
def quatDCM(q)
Definition: pose.py:251
def qexp(v)
Definition: pose.py:760
def quat2eulerArray(q)
Definition: pose.py:134
def euler2quat(euler)
Definition: pose.py:147
def vectorRotateBodyToInertial(vIn, eRot)
Definition: pose.py:601
def vectorRotateInertialToBody(vIn, eRot)
Definition: pose.py:589
def acc2AttAndBias(acc)
Definition: pose.py:674
def DCMeulerToPsi(A, phi, the)
Definition: pose.py:235
def vectorEuler(v)
Definition: pose.py:640
def euler2quatArray(euler)
Definition: pose.py:168
def quatInit()
Definition: pose.py:18
def qinv(q)
Definition: pose.py:770
def quatInv(q)
Definition: pose.py:29
def meanOfQuat(q)
Definition: pose.py:784
def DCMquat(mat)
Definition: pose.py:268
def DCMpsi(A)
Definition: pose.py:202
def quatConj(q)
Definition: pose.py:25


inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04