modelling and simulation of marine craft kinematics
TRANSCRIPT
Modelling and Simulation of Marine Craft
KINEMATICSEdin OmerdicSenior Research FellowMobile & Marine Robotics Research CentreUniversity of Limerick
Mobile & Marine Robotics Research Centre
University of Limerick
Outline
KinematicsReference Frames
ECEF Frame
NED Frame
BODY Frame
UNITY Frame
UTM
Transformations BODY – NEDEuler Angles
Quaternions
Quaternions from Euler Angles
Euler Angles from Quaternions
Transformations ECEF – NEDLat-Lon from ECEF Coordinates
ECEF Coordinates from Lat-Lon
Mobile & Marine Robotics Research Centre
University of Limerick
GNC Signal Flow
GNC Signal Flow
Weather routing
program
Trajectory
Generator
Motion Control
System
Control
Allocation
Fusion Marine Craft INS (GNSS + AHRS)
OA Sensors
(Radar, Cameras, etc.)
Observer
Weather
data
Waypoints
Guidance
System
Control
System
Obstacle
Avoidance
Module
Estimated
positions and
velocities Navigation
System
Waves, wind
and ocean
currents
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsReference Frames
ECEF Frame
Symbol 𝐞 = (𝒙𝐞, 𝒚𝐞, 𝒛𝐞)
Definition The Earth-centered Earth-fixed frame which rotate around North-South axis with angular speed 𝑤𝑒 = 7.2921 ∗ 10−5 𝑟𝑎𝑑/𝑠
Type Right-handed
Axes 𝑥e (from origin through intersection of Prime Meridian and Equator)
𝑦e𝑧e (from origin through North pole)
Origin Location of ECEF origin 𝑜e is fixed to the Centre of the Earth.
Assumption For marine craft moving at relatively low speed the Earth rotation can be neglected and e can be considered to be inertial, such
that Newton's laws apply.
Application Typically used for global guidance, navigation and control (for example, to describe motion and location of ships in transit between
continents).
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsReference Frames
NED Frame
Symbol 𝒏 = (𝒙𝒏, 𝒚𝒏, 𝒛𝒏)
Definition Tangent plane on the surface of Earth reference ellipsoid WGS 1984.
Type Right-handed
Axes 𝑥𝑛= North
𝑦𝑛= East
𝑧𝑛= Downwards, normal to the surface.
Origin Location of NED origin 𝑜𝑛 relative to ECEF frame 𝒆 = (𝒙𝒆, 𝒚𝒆, 𝒛𝒆) is determined using Lat and Lon.
Assumption 𝑛 is inertial, such that Newton's laws apply.
Application The position and orientation of the craft are described relative to 𝑛 .
𝑥𝑒
𝑦𝑒
𝑧𝑒
Equator
𝑥𝑛𝑦𝑛
𝑧𝑛
𝜆
𝜑
𝜆 − 𝐿𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝜑 − 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsReference Frames
BODY Frame
Symbol 𝒃 = (𝒙𝒃, 𝒚𝒃, 𝒛𝒃)
Definition Moving frame fixed to the craft.
Type Right-handed
Axes 𝑥𝑏= longitudinal axis (directed toward front);
𝑦𝑛= transversal axis (directed toward starboard);
𝑧𝑛= normal axis (directed toward bottom)
Origin Location of BODY origin 𝑜𝑏 is usually chosen to coincide with a point midships in
the water line for marine crafts.
Application The linear and angular velocities of the vessel are described relative to 𝑏 .
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsReference Frames
UNITY Frame
Symbol 𝒖 = (𝒙𝒖, 𝒚𝒖, 𝒛𝒖)
Definition Frame used for 3D visualisation in Unity 3D.
Type Left-handed
Axes 𝑥𝑢= East
𝑦𝑢= Up
𝑧𝑢= North.
Origin Location of UNITY origin 𝑜𝑢 relative to ECEF frame 𝒆 = (𝒙𝒆, 𝒚𝒆, 𝒛𝒆) is determined using Lat and Lon.
Application The position and orientation of all vessels are sent to Unity 3D for visualisation.
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsReference Frames
UTM
Symbol 𝑼𝑻𝑴 = (𝒙𝑼𝑻𝑴, 𝒚𝑼𝑻𝑴, 𝒛𝑼𝑻𝑴)
Definition The UTM system divides the Earth into 60 zones and uses a secant transverse Mercator projection in
each zone (conformal projection) to give location on the surface of the Earth. Each zone is segmented
into 20 latitude bands.
Type Right-handed
Axes 𝑥𝑈𝑇𝑀= Northing
𝑦𝑈𝑇𝑀= Easting
𝑧𝑈𝑇𝑀= Downwards, normal to the surface.
Origin Location of UTM origin 𝑜𝑈𝑇𝑀 is shifted such that Northing and Easting coordinates are always
positive. For this purpose artificial offset False Easting is added in Northern hemisphere, while both
False Easting and False Northing are used in Southern hemisphere. In vertical plane the UTM origin is
at sea level.
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsReference Frames
UTM (cont.)
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations between Frames
Notation
𝒑𝑏/𝑈𝑇𝑀𝑈𝑇𝑀 =
𝑥𝑈𝑇𝑀𝑦𝑈𝑇𝑀𝑧𝑈𝑇𝑀
Position of vessel 𝑜𝑏 in UTM
𝒑𝑏/𝑛𝑛 =
𝑥𝑛𝑦𝑛𝑧𝑛
Position of vessel 𝑜𝑏 in NED
𝒗𝑏/𝑛𝑏 =
𝑢𝑣𝑤
Linear velocity of 𝑜𝑏 with
respect to {n} expressed in {b}𝒗𝑏/𝑛𝑛 Linear velocity of 𝑜𝑏 with
respect to {n} expressed in {n}
𝜽𝑛𝑏 =𝑅𝑃𝑌
Attitude (Euler angles) between
{n} and {b} 𝒘𝑏/𝑛𝑏 =
𝑝𝑞𝑟
Angular velocity of 𝑏 with
respect to {n} expressed in {b}
𝒑𝑛/𝑈𝑇𝑀𝑈𝑇𝑀 Position of NED origin 𝑜𝑛 in
UTM ሶ𝛉𝑛𝑏 =ሶ𝑅ሶ𝑃ሶ𝑌
Euler rate vector
𝑹𝑏𝑛 𝜃𝑛𝑏 Transformation matrix of linear
velocity vector 𝒗𝑏/𝑛𝑏 from {b} to
𝒗𝑏/𝑛𝑛 in {n}
𝑻𝜃 𝜽𝒏𝒃 Transformation matrix of
angular velocity vector 𝒘𝑏/𝑛𝑏
from {b} to Euler rate vectorሶ𝛉𝑛𝑏 in {n}
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations between Frames
Euler’s Rotation Theorem:
Geometry perspective
Any displacement of a rigid-body in 3D, such that a point on the rigid body remains fixed, is equivalent to a single rotation about some axis that passes through the fixed point.
Linear Algebra perspective
Any two Cartesian coordinate systems in 3D with a common origin are related by a rotation about some fixed axis passing through the origin.
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDEuler Angles
Linear Velocity Transformation
ሶ𝒑 Τ𝑏 𝑛𝑛 = 𝒗 Τ𝑏 𝑛
𝑛 = 𝑹𝑏𝑛 𝜽𝑛𝑏 𝒗 Τ𝑏 𝑛
𝑏 = 𝑹𝑧,𝑌𝑹𝑦,𝑃𝑹𝑥,𝑅𝒗 Τ𝑏 𝑛𝑏
𝑹𝑥,𝑅 =1 0 00 𝑐𝑅 −𝑠𝑅0 𝑠𝑅 𝑐𝑅
, 𝑹𝑦,𝑃 =𝑐𝑃 0 𝑠𝑃0 1 0
−𝑠𝑃 0 𝑐𝑃, 𝑹𝑧,𝑌=
𝑐𝑌 −𝑠𝑌 0𝑠𝑌 𝑐𝑌 00 0 1
𝑹𝑏𝑛 𝜽𝑛𝑏 =
𝑐𝑌𝑐𝑃 −𝑠𝑌𝑐𝑅 + 𝑐𝑌𝑠𝑃𝑠𝑅 𝑠𝑌𝑠𝑅 + 𝑐𝑌𝑐𝑅𝑠𝑃𝑠𝑌𝑐𝑃 𝑐𝑌𝑐𝑅 + 𝑠𝑌𝑠𝑃𝑠𝑅 −𝑐𝑌𝑠𝑅 + 𝑠𝑃𝑠𝑌𝑐𝑅−𝑠𝑃 𝑐𝑃𝑠𝑅 𝑐𝑃𝑐𝑅
𝑹𝑏𝑛 𝜽𝑛𝑏
−1 = 𝑹𝑏𝑛 𝜽𝑛𝑏
𝑇
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDEuler Angles
Angular Velocity Transformation
ሶ𝜽𝑛𝑏 = 𝑻𝜃 𝜽𝒏𝒃 𝒘 Τ𝑏 𝑛𝑏 =
1 𝑠𝑅𝑡𝑃 𝑐𝑅𝑡𝑃0 𝑐𝑅 −𝑠𝑅0 𝑠 Τ𝑅 𝑐 𝑃 𝑐 Τ𝑅 𝑐 𝑃
𝒘 Τ𝑏 𝑛𝑏
Singularity for P=±90°
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDEuler Angles
6 DOF Kinematic Equations
ሶ𝒑 Τ𝑏 𝑛𝑛
ሶ𝜽𝑛𝑏=
𝑹𝑏𝑛 𝜽𝑛𝑏 𝟎3×3𝟎3×3 𝑻𝜃 𝜽𝒏𝒃
𝒗 Τ𝑏 𝑛𝑏
𝒘 Τ𝑏 𝑛𝑏
ሶ𝜼 = 𝑱𝜣 𝜼 𝝂
Simulation ModelAttitude representation: Euler Angles
r
q
p
w
v
u
b
nb
b
nb
/
/
w
vv
nb
n
nb
θ
pη
/
Y
P
R
z
y
x
n
n
n
nb
n
nb
θ
pη /
Gain Integrator
0
00 /
nb
n
nb
θ
pη
KINEMATICS
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Basis: 1,i,j,k𝑯 ≡ ℝ4
Addition:
𝑞 = 𝑎𝟏 + 𝑏𝒊 + 𝑐𝒋 + 𝑑𝒌
Scalar part
Vector part
“Real” quaternions: 𝑎𝟏 + 0𝒊 + 0𝒋 + 0𝒌
“Pure” quaternions: 0𝟏 + 𝑏𝒊 + 𝑐𝒋 + 𝑑𝒌
𝑎1𝟏 + 𝑏1𝒊 + 𝑐1𝒋 + 𝑑1𝒌 + 𝑎2𝟏 + 𝑏2𝒊 + 𝑐2𝒋 + 𝑑2𝒌= 𝑎1 + 𝑎2 𝟏 + 𝑏1 + 𝑏2 𝒊 + 𝑐1 + 𝑐2 𝒋 + 𝑑1 + 𝑑2 𝒌
Scalar Multiplication: 𝛼 𝑎𝟏 + 𝑏𝒊 + 𝑐𝒋 + 𝑑𝒌 = 𝛼𝑎𝟏 + 𝛼𝑏𝒊 + 𝛼𝑐𝒋 + 𝛼𝑑𝒌
Quaternion Multiplication:
𝑎1𝟏 + 𝑏1𝒊 + 𝑐1𝒋 + 𝑑1𝒌 𝑎2𝟏 + 𝑏2𝒊 + 𝑐2𝒋 + 𝑑2𝒌= 𝑎1𝑎2 − 𝑏1𝑏2 − 𝑐1𝑐2 − 𝑑1𝑑2 𝟏+ 𝑎1𝑏2 + 𝑏1𝑎2 + 𝑐1𝑑2 − 𝑑1𝑐2 𝒊+ 𝑎1𝑐2 − 𝑏1𝑑2 + 𝑐1𝑎2 + 𝑑1𝑏2 𝒋+ 𝑎1𝑑2 + 𝑏1𝑐2 − 𝑐1𝑏2 + 𝑑1𝑎2 𝒌
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Addition:
Scalar part
Vector part
𝑟1, Ԧ𝑣1 + 𝑟2, Ԧ𝑣2 = 𝑟1 + 𝑟2, Ԧ𝑣1 + Ԧ𝑣2
Scalar Multiplication:
Quaternion Multiplication:
𝑞 = 𝑟, Ԧ𝑣
𝛼 𝑟, Ԧ𝑣 = 𝛼𝑟, 𝛼 Ԧ𝑣
𝑟1, Ԧ𝑣1 𝑟2, Ԧ𝑣2 = 𝑟1𝑟2 − Ԧ𝑣1 ∙ Ԧ𝑣2, 𝑟1 Ԧ𝑣2 + 𝑟2 Ԧ𝑣1 + Ԧ𝑣1 × Ԧ𝑣2
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Conjugate:
Scalar part
Vector part
𝑞∗ = 𝑟, − Ԧ𝑣 = 𝑟𝟏 − 𝑣𝑥𝒊 − 𝑣𝑦𝒋 − 𝑣𝑧𝒌
Norm:
𝑞 = 𝑟, Ԧ𝑣 = 𝑟𝟏 + 𝑣𝑥𝒊 + 𝑣𝑦𝒋 + 𝑣𝑧𝒌
𝑞 = 𝑞𝑞∗ = 𝑟2 + Ԧ𝑣 2 = 𝑟2 + 𝑣𝑥2 + 𝑣𝑦
2 + 𝑣𝑧2
𝑞𝑞∗ = 𝑟, Ԧ𝑣 𝑟, − Ԧ𝑣 = 𝑟2 + Ԧ𝑣 2, 0 = 𝑟2 + 𝑣𝑥2 + 𝑣𝑦
2 + 𝑣𝑧2 𝟏 + 0𝒊 + 0𝒋 + 0𝒌
𝑞1𝑞2∗ = 𝑞2
∗𝑞1∗
𝑞∗𝑞 = 𝑟,− Ԧ𝑣 𝑟, Ԧ𝑣 = 𝑟2 + Ԧ𝑣 2, 0 = 𝑞𝑞∗
𝑝𝑞 2 = 𝑝𝑞 𝑝𝑞 ∗ = 𝑝𝑞𝑞∗𝑝∗ = 𝑝 𝑞 2𝑝∗ = 𝑝𝑝∗ 𝑞 2 = 𝑝 2 𝑞 2
Inverse: 𝑞−1 =𝑞∗
𝑞 2
𝑞−1𝑞 = 𝑞𝑞−1 = 1
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Unit Quaternion: 𝑞 = 1 ⇒ 𝑟2 + Ԧ𝑣 2 = 1 ⇒ ∃𝜃 ∈ 0, 𝜋 : cos 𝜃 = 𝑟, sin 𝜃 = Ԧ𝑣
𝑞 = 1 ⇒ 𝑞 = 𝑟, Ԧ𝑣 = r + Ԧ𝑣Ԧ𝑣
Ԧ𝑣= cos 𝜃 + sin 𝜃 Ԧො𝑣 = 𝑒
0,𝜃 ො𝑣cos 𝜃 sin 𝜃
Ԧො𝑣
Polar Decomposition: 𝑞 = 𝑞
𝑞
𝑞= 𝑞 cos 𝜃 + sin 𝜃 Ԧො𝑣 = 𝑞 𝑒
0,𝜃 ො𝑣
Unit Quaternion Unit
Quaternion
Unit Quaternion
Power: 𝑞𝑝 = 𝑞 𝑝𝑒0, 𝑝𝜃 ො𝑣
= 𝑞 𝑝 cos 𝑝𝜃 + sin 𝑝𝜃 Ԧො𝑣
Euler Formula(General Case): 𝑒 𝑟,𝑣 = 𝑒
𝑟,0 + 0, 𝑣 ො𝑣= 𝑒 𝑟,0 𝑒
0,𝜃 ො𝑣= 𝑒𝑟𝑒
0,𝜃 ො𝑣= 𝑒𝑟 cos 𝜃 , sin 𝜃 Ԧො𝑣
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Euler Formula (“Pure” Quaternion):𝑞 = 0, Ԧ𝑣
𝑞2 = 0, Ԧ𝑣 0, Ԧ𝑣 = − Ԧ𝑣 2, 0
⋮
𝑞3 = − Ԧ𝑣 2, 0 0, Ԧ𝑣 = 0,− Ԧ𝑣 2 Ԧ𝑣
𝑞4 = 0,− Ԧ𝑣 2 Ԧ𝑣 0, Ԧ𝑣 = Ԧ𝑣 4, 0
𝑞5 = Ԧ𝑣 4, 0 0, Ԧ𝑣 = 0, Ԧ𝑣 4 Ԧ𝑣
𝑞6 = 0, Ԧ𝑣 4 Ԧ𝑣 0, Ԧ𝑣 = − Ԧ𝑣 6, 0
𝑞7 = − Ԧ𝑣 6, 0 0, Ԧ𝑣 = 0,− Ԧ𝑣 6 Ԧ𝑣
𝑒𝑞 = 1 + 𝑞 +𝑞2
2!+𝑞3
3!+𝑞4
4!+𝑞5
5!+𝑞6
6!+𝑞7
7!⋯
𝑒 0,𝑣 = 1,0 + 0, Ԧ𝑣 +− Ԧ𝑣 2, 0
2!+
0,− Ԧ𝑣 2 Ԧ𝑣
3!+
Ԧ𝑣 4, 0
4!+
0, Ԧ𝑣 4 Ԧ𝑣
5!⋯
𝑒 0,𝑣 = 1 −Ԧ𝑣 2
2!+
Ԧ𝑣 4
4!⋯ , 1 −
Ԧ𝑣 2
3!+
Ԧ𝑣 4
5!⋯
Ԧ𝑣
Ԧ𝑣= cos Ԧ𝑣 , sin Ԧ𝑣 Ԧො𝑣
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Special case: Rotation About Perpendicular AxisԦ𝑣 , = cos𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣
ො𝑛 × Ԧ𝑣
Ԧ𝑣
Ԧ𝑣 ,
𝜃
0ො𝑛
cos 𝜃 Ԧ𝑣
sin 𝜃 ො𝑛 × Ԧ𝑣
𝑣 = 0, Ԧ𝑣
𝑛 = 0, ො𝑛
𝑣 , = 0, Ԧ𝑣 , = 0, cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣 = cos 𝜃 , sin 𝜃 ො𝑛 0, Ԧ𝑣 = 𝑒0,𝜃 ො𝑛
0, Ԧ𝑣 = 𝑒0,𝜃 ො𝑛
𝑣
𝑛𝑣 = 0, ො𝑛 0, Ԧ𝑣 = 0, ො𝑛 × Ԧ𝑣
Solution using Quaternions:
𝑛2 = 0, ො𝑛 0, ො𝑛 = −1, 0
𝑣 ,= 𝑒0,𝜃 ො𝑛
𝑣
ො𝑛
0Ԧ𝑣
Ԧ𝑣 ,
𝜃
ො𝑛 × Ԧ𝑣
ො𝑛 = 1
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Ԧ𝑣⊥, = cos𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥
ො𝑛 × Ԧ𝑣⊥
Ԧ𝑣⊥
Ԧ𝑣⊥,
𝜃
0ො𝑛
cos 𝜃 Ԧ𝑣⊥
sin 𝜃 ො𝑛 × Ԧ𝑣⊥
Ԧ𝑣 , = Ԧ𝑣∥, + Ԧ𝑣⊥
, = Ԧ𝑣∥ + cos 𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥
Rotation About General Axis
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
𝜃Ԧ𝑣⊥
Ԧ𝑣∥ = Ԧ𝑣∥,
Ԧ𝑣⊥,
ො𝑛 × Ԧ𝑣⊥
cos 𝜃 Ԧ𝑣⊥
sin 𝜃 ො𝑛 × Ԧ𝑣⊥
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Vector Projection onto Line
Ԧ𝑣
𝑏Ԧ𝑣⊥
Ԧ𝑣∥
Ԧ𝑣 = Ԧ𝑣∥ + Ԧ𝑣⊥ ⇒ Ԧ𝑣⊥ = Ԧ𝑣 − Ԧ𝑣∥ = Ԧ𝑣 − 𝛼𝑏
𝑏 ⊥ Ԧ𝑣⊥ ⇒ 𝑏 ∙ Ԧ𝑣⊥ = 0
𝑏 ∙ Ԧ𝑣 − 𝛼𝑏 = 0
Ԧ𝑣∥ = 𝛼𝑏
𝑏 ∙ Ԧ𝑣 − 𝛼 𝑏 ∙ 𝑏 = 0
𝛼 =𝑏 ∙ Ԧ𝑣
𝑏 ∙ 𝑏=𝑏𝑇 Ԧ𝑣
𝑏𝑇𝑏
Ԧ𝑣∥ = 𝛼𝑏 =𝑏 ∙ Ԧ𝑣
𝑏 ∙ 𝑏𝑏 =
𝑏𝑇 Ԧ𝑣
𝑏𝑇𝑏𝑏 = 𝑏
𝑏𝑇 Ԧ𝑣
𝑏𝑇𝑏=𝑏𝑏𝑇
𝑏𝑇𝑏Ԧ𝑣
Projection Matrix
Scalar
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Special Case:
Ԧ𝑣
𝑏 = ො𝑛
Ԧ𝑣⊥
Ԧ𝑣∥
Ԧ𝑣∥ =ො𝑛ො𝑛𝑇
ො𝑛𝑇 ො𝑛Ԧ𝑣 =
ො𝑛 ො𝑛𝑇
ො𝑛2 Ԧ𝑣 = ො𝑛 ො𝑛𝑇 Ԧ𝑣
𝑏 = ො𝑛 ො𝑛 = 1
Ԧ𝑣∥ =ො𝑛 ∙ Ԧ𝑣
ො𝑛 ∙ ො𝑛ො𝑛 =
ො𝑛 ∙ Ԧ𝑣
ො𝑛2 ො𝑛 = ො𝑛 ∙ Ԧ𝑣 ො𝑛
Projection Matrix
Scalar
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Special Case:
Ԧ𝑣
Ԧ𝑣⊥
Ԧ𝑣∥
Ԧ𝑞 = sin𝜃
2
𝑏 = Ԧ𝑞 = sin𝜃
2ො𝑛
𝑏 = Ԧ𝑞 = sin𝜃
2ො𝑛
Ԧ𝑣∥ =Ԧ𝑞 Ԧ𝑞𝑇
Ԧ𝑞𝑇 Ԧ𝑞Ԧ𝑣 =
Ԧ𝑞 Ԧ𝑞𝑇
Ԧ𝑞 2 Ԧ𝑣 =Ԧ𝑞 Ԧ𝑞𝑇
sin2𝜃2
Ԧ𝑣
Ԧ𝑣∥ =Ԧ𝑞 ∙ Ԧ𝑣
Ԧ𝑞 ∙ Ԧ𝑞Ԧ𝑞 =
Ԧ𝑞 ∙ Ԧ𝑣
Ԧ𝑞 2 Ԧ𝑞 =Ԧ𝑞 ∙ Ԧ𝑣
sin2𝜃2
Ԧ𝑞
Projection Matrix
Scalar
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Ԧ𝑣 , = Ԧ𝑣∥, + Ԧ𝑣⊥
, = Ԧ𝑣∥ + cos 𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥ = Ԧ𝑣∥ + cos𝜃 Ԧ𝑣 − Ԧ𝑣∥ + sin 𝜃 ො𝑛 × Ԧ𝑣 − Ԧ𝑣∥
= 1 − cos𝜃 Ԧ𝑣∥ + cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣 − sin 𝜃 ො𝑛 × Ԧ𝑣∥0
Rodrigues Rotation Formula: Ԧ𝑣 , = cos 𝜃 Ԧ𝑣 + 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 + sin 𝜃 ො𝑛 × Ԧ𝑣
Ԧ𝑣∥ = Ԧ𝑣 ∙ ො𝑛 ො𝑛 Ԧ𝑣∥
Rotation About General Axis
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
𝜃Ԧ𝑣⊥
Ԧ𝑣∥ = Ԧ𝑣∥,
Ԧ𝑣⊥,
ො𝑛 × Ԧ𝑣⊥
1 − cos 𝜃 Ԧ𝑣∥
cos 𝜃 Ԧ𝑣
sin 𝜃 ො𝑛 × Ԧ𝑣
Ԧ𝑣⊥, = cos𝜃 Ԧ𝑣⊥ + sin 𝜃 ො𝑛 × Ԧ𝑣⊥
ො𝑛 × Ԧ𝑣⊥
Ԧ𝑣⊥
Ԧ𝑣⊥,
𝜃
0ො𝑛
cos 𝜃 Ԧ𝑣⊥
sin 𝜃 ො𝑛 × Ԧ𝑣⊥
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Rotation About General Axis: Solution using Quaternions
𝑣 = 0, Ԧ𝑣
𝑛 = 0, ො𝑛
𝑣∥ = 0, Ԧ𝑣∥
𝑣⊥ = 0, Ԧ𝑣⊥Lema 2: 𝑒
0,𝜃 ො𝑛𝑣⊥ = 𝑣⊥𝑒
0,−𝜃 ො𝑛
Lema 1: 𝑒0,𝜃 ො𝑛
𝑣∥ = 𝑣∥𝑒0,𝜃 ො𝑛
Ԧ𝑣 , = Ԧ𝑣∥, + Ԧ𝑣⊥
, = Ԧ𝑣∥ + Ԧ𝑣⊥,
𝑣 , = 0, Ԧ𝑣 ,
𝑣 , = 0, Ԧ𝑣 , = 0, Ԧ𝑣∥, + Ԧ𝑣⊥
, = 0, Ԧ𝑣∥ + Ԧ𝑣⊥,
𝑣 , = 𝑒0,𝜃2ො𝑛𝑒
0,−𝜃2ො𝑛𝑣∥ + 𝑒
0,𝜃2ො𝑛𝑒
0,𝜃2ො𝑛𝑣⊥ = 𝑒
0,𝜃2ො𝑛𝑣∥𝑒
0,−𝜃2ො𝑛+ 𝑒
0,𝜃2ො𝑛𝑣⊥𝑒
0,−𝜃2ො𝑛
𝑣 , = 𝑣∥ + 𝑣⊥, = 𝑣∥ + 𝑒
0,𝜃 ො𝑛𝑣⊥
Lema 1 Lema 2
𝑣 , = 𝑒0,𝜃2ො𝑛
𝑣∥ + 𝑣⊥ 𝑒0,−
𝜃2ො𝑛
= 𝑒0,𝜃2ො𝑛𝑣𝑒
0,−𝜃2ො𝑛
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
𝜃Ԧ𝑣⊥
Ԧ𝑣∥ = Ԧ𝑣∥,
Ԧ𝑣⊥,
Ԧ𝑞 = sin𝜃
2ො𝑛
Ԧ𝑣∥ =Ԧ𝑞 ∙ Ԧ𝑣
Ԧ𝑞 ∙ qԦ𝑞
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Rodrigues Rotation Formula:
Ԧ𝑣 , = 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 + Ԧ𝑣 + cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣
= 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 + 1 − cos 𝜃 + cos 𝜃 Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣
Ԧ𝑣∥
− Ԧ𝑣⊥
Ԧ𝑣 , = Ԧ𝑣 + 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 + sin 𝜃 ො𝑛 × Ԧ𝑣
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
𝜃Ԧ𝑣⊥
Ԧ𝑣∥ = Ԧ𝑣∥,
Ԧ𝑣⊥,
1 − cos 𝜃 − Ԧ𝑣⊥
sin 𝜃 ො𝑛 × Ԧ𝑣
ො𝑛 × Ԧ𝑣 ≔ 𝑺 𝒏 𝐯
ො𝑛 × ො𝑛 × Ԧ𝑣 = Ԧ𝑣 ∙ ො𝑛 ො𝑛 − Ԧ𝑣 = − Ԧ𝑣⊥
𝑺 𝒏 𝑺 𝒏 𝐯 = 𝑺2 𝒏 𝐯 = 𝒏𝒏𝑻 − 𝑰𝟑×𝟑 𝐯
= 𝒏𝒏𝑻 𝐯 − 𝑰𝟑×𝟑𝐯 = 𝐯∥ − 𝐯 = −𝐯⊥
𝐯, = 𝑰𝟑×𝟑 + 1 − cos 𝜃 𝑺2 𝒏 + sin 𝜃𝑺 𝒏 𝐯
𝑹𝒏,𝜃
Ԧ𝑣 , = cos𝜃 Ԧ𝑣 + 1 − cos 𝜃 Ԧ𝑣 ∙ ො𝑛 ො𝑛 + sin 𝜃 ො𝑛 × Ԧ𝑣
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Rotation Formula using Quaternions:
𝑞 = 𝑒0,𝜃2ො𝑛
= cos𝜃
2, sin
𝜃
2ො𝑛 = 𝑞0, Ԧ𝑞 , 𝑞0
2+ Ԧ𝑞 2 = 1
𝑣 , = 𝑒0,𝜃2ො𝑛
𝑣∥ + 𝑣⊥ 𝑒0,−
𝜃2ො𝑛
= 𝑒0,𝜃2ො𝑛𝑣𝑒
0,−𝜃2ො𝑛
= 𝑞𝑣𝑞∗
𝑣 , = 0, 𝑰𝟑×𝟑 + 1 − cos 𝜃 𝑺2 𝒏 + sin 𝜃𝑺 𝒏 Ԧ𝑣 =
= 0, 𝑰𝟑×𝟑 + 2sin2𝜃
2𝑺2 𝒏 + 2 sin
𝜃
2cos
𝜃
2𝑺 𝒏 Ԧ𝑣
𝑣 , = 0, 𝑰𝟑×𝟑 + 2𝑺2 sin𝜃
2𝒏 + 2 cos
𝜃
2𝑺 sin
𝜃
2𝒏 Ԧ𝑣
𝑹𝑏𝑛 𝒒 = 𝑹 𝑞0,𝑞
𝑣 , = 0, 𝑰𝟑×𝟑 + 2𝑺2 Ԧ𝑞 + 2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
𝜃Ԧ𝑣⊥
Ԧ𝑣∥ = Ԧ𝑣∥,
Ԧ𝑣⊥,
2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣
2𝑺2 Ԧ𝑞 Ԧ𝑣
Ԧ𝑞 = sin𝜃
2ො𝑛
𝑺2 Ԧ𝑞 = Ԧ𝑞 Ԧ𝑞𝑇 − Ԧ𝑞 2𝑰𝟑×𝟑
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
𝑣 , = 𝑒0,𝜃2ො𝑛
𝑣∥ + 𝑣⊥ 𝑒0,−
𝜃2ො𝑛
=
= 𝑒0,𝜃2ො𝑛𝑣∥ 𝑒
0,−𝜃2ො𝑛+ 𝑒
0,𝜃2ො𝑛𝑣⊥ 𝑒
0,−𝜃2ො𝑛
𝑣 , = 𝑒0,𝜃 ො𝑛
𝑣∥ + 𝑣⊥ = 𝑒0,𝜃 ො𝑛
𝑣∥ + 𝑒0,𝜃 ො𝑛
𝑣⊥Wrong Formula:
Correct Formula:
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
𝜃Ԧ𝑣⊥
Ԧ𝑣∥,
Ԧ𝑣⊥,
Ԧ𝑣∥Ԧ𝑣∥,
Ԧ𝑣⊥,
Ԧ𝑝∥,
Ԧ𝑝⊥,
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
Ԧ𝑣⊥
Ԧ𝑣∥
Step 1:
Ԧ𝑝⊥,
Ԧ𝑝∥,
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
Ԧ𝑣⊥
Ԧ𝑣⊥,
Ԧ𝑣∥, = Ԧ𝑣∥
Step 2:
Ԧ𝑝⊥,
Ԧ𝑝∥,
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Rotation Formula using Quaternions:
𝑞 = 𝑒0,𝜃2ො𝑛
= cos𝜃
2, sin
𝜃
2ො𝑛 = 𝑞0, Ԧ𝑞 , 𝑞0
2+ Ԧ𝑞 2 = 1
𝑣 , = 𝑒0,𝜃2ො𝑛
𝑣∥ + 𝑣⊥ 𝑒0,−
𝜃2ො𝑛
= 𝑒0,𝜃2ො𝑛𝑣𝑒
0,−𝜃2ො𝑛
= 𝑞𝑣𝑞∗
Quaternion Rotation Operator:
𝑣 , = 0, 𝑞02 − Ԧ𝑞 2 𝑰𝟑×𝟑 + 2Ԧ𝑞 Ԧ𝑞𝑇 + 2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣
𝑣 , = 𝐿𝑞 𝑣 ≔ 𝑞𝑣𝑞∗ = 𝑞0, Ԧ𝑞 0, Ԧ𝑣 𝑞0, − Ԧ𝑞 = 0, 𝑞02 − Ԧ𝑞 2 Ԧ𝑣 + 2 Ԧ𝑞 ∙ Ԧ𝑣 Ԧ𝑞 + 2𝑞0 Ԧ𝑞 × Ԧ𝑣
ො𝑛
0
Ԧ𝑣
Ԧ𝑣 ,
𝜃Ԧ𝑣⊥
Ԧ𝑣∥ = Ԧ𝑣∥,
Ԧ𝑣⊥,
2𝑞0𝑺 Ԧ𝑞 Ԧ𝑣
2 Ԧ𝑞 Ԧ𝑞𝑇 Ԧ𝑣 = 2 Ԧ𝑞 ∙ Ԧ𝑣 Ԧ𝑞
𝑞02 − Ԧ𝑞 2 Ԧ𝑣
Ԧ𝑞 = sin𝜃
2ො𝑛
𝑞02 − Ԧ𝑞 2 Ԧ𝑣 = 𝑞0
2 + Ԧ𝑞 2 − 2 Ԧ𝑞 2 Ԧ𝑣 = 1 − 2sin2𝜃
2Ԧ𝑣 = cos 𝜃 Ԧ𝑣
1 − cos 𝜃 Ԧ𝑣∥ = 2sin2𝜃
2
Ԧ𝑞 ∙ Ԧ𝑣
Ԧ𝑞 ∙ qԦ𝑞 = 2sin2
𝜃
2
Ԧ𝑞 ∙ Ԧ𝑣
Ԧ𝑞 2Ԧ𝑞 =
= 2sin2𝜃
2
Ԧ𝑞 ∙ Ԧ𝑣
sin2𝜃2
Ԧ𝑞 = 2 Ԧ𝑞 ∙ Ԧ𝑣 Ԧ𝑞 = 2 Ԧ𝑞 Ԧ𝑞𝑇 Ԧ𝑣 = 2 Ԧ𝑞 Ԧ𝑞𝑇 Ԧ𝑣
Scalar Projection Matrix
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Linear Velocity Transformation
ሶ𝒑 Τ𝑏 𝑛𝑛 = 𝒗 Τ𝑏 𝑛
𝑛 = 𝑹𝑏𝑛 𝒒 𝒗 Τ𝑏 𝑛
𝑏
𝑹𝑏𝑛 𝒒 = 𝑹 𝑞0,𝑞 = 𝑰𝟑×𝟑 + 2𝑺2 Ԧ𝑞 + 2𝑞0𝑺 Ԧ𝑞
𝑹𝑏𝑛 𝒒 =
1 − 2 𝑞22 + 𝑞3
2 2 𝑞1𝑞2 − 𝑞3𝑞0 2 𝑞1𝑞3 + 𝑞2𝑞02 𝑞1𝑞2 + 𝑞3𝑞0 1 − 2 𝑞3
2 + 𝑞12 2 𝑞2𝑞3 − 𝑞1𝑞0
2 𝑞1𝑞3 − 𝑞2𝑞0 2 𝑞2𝑞3 + 𝑞1𝑞0 1 − 2 𝑞12 + 𝑞2
2
𝑞 = 𝑞0, Ԧ𝑞 = 𝑞0𝟏 + 𝑞1𝒊 + 𝑞2𝒋 + 𝑞3𝒌
𝑞0 = cos𝜃
2
Ԧ𝑞 = sin𝜃
2ො𝑛
𝑹𝑏𝑛 𝒒 −1 = 𝑹𝑏
𝑛 𝒒 𝑇
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
Angular Velocity Transformation
ሶ𝑹𝒃𝑛 𝒒 = 𝑹𝑏
𝑛 𝒒 𝑺 𝒘 Τ𝑏 𝑛𝑏 ⇒ ሶ𝒒 = 𝑻𝒒 𝒒 𝒘 Τ𝑏 𝑛
𝑏
𝑞 = 𝑞0, Ԧ𝑞 = 𝑞0𝟏 + 𝑞1𝒊 + 𝑞2𝒋 + 𝑞3𝒌
𝑞0 = cos𝜃
2
Ԧ𝑞 = sin𝜃
2ො𝑛
𝑻𝒒𝑇 𝒒 𝑻𝒒 𝒒 =
1
4𝑰𝟑×𝟑
𝑻𝒒 𝒒 =1
2
−𝑞1𝑞0
−𝑞2 −𝑞3−𝑞3 𝑞2
𝑞3−𝑞2
𝑞0𝑞1
−𝑞1𝑞0
𝑻𝒒 𝒒 =1
2
−Ԧ𝑞𝑇
𝑞0𝑰𝟑×𝟑 + 𝑺 Ԧ𝑞
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions versus Euler Angles
Angular Velocity Transformation
𝑻𝒒 𝒒 =1
2
−𝑞1𝑞0
−𝑞2 −𝑞3−𝑞3 𝑞2
𝑞3−𝑞2
𝑞0𝑞1
−𝑞1𝑞0
𝑻𝜃 𝜽𝒏𝒃 =1 𝑠𝑅𝑡𝑃 𝑐𝑅𝑡𝑃0 𝑐𝑅 −𝑠𝑅0 𝑠 Τ𝑅 𝑐 𝑃 𝑐 Τ𝑅 𝑐 𝑃
Singularity for P=±90°
Quaternions Euler Angles
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions
6 DOF Kinematic Equations
ሶ𝒑 Τ𝑏 𝑛𝑛
ሶ𝒒=
𝑹𝑏𝑛 𝒒 𝟎3×3𝟎4×3 𝑻𝒒 𝒒
𝒗 Τ𝑏 𝑛𝑏
𝒘 Τ𝑏 𝑛𝑏
ሶ𝜼 = 𝑱𝒒 𝜼 𝝂
Simulation ModelAttitude representation: Quaternions
r
q
p
w
v
u
b
nb
b
nb
/
/
w
vv
q
pη
n
nb /
3
2
1
0
/
q
q
q
q
z
y
x
n
n
n
n
nb
q
pη
Gain Integrator
0
00 /
q
pη
n
nb
KINEMATICS
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDQuaternions from Euler Angles
Euler Angles → Quaternions
𝒒 =
𝑞0𝑞1𝑞2𝑞3
=
𝑐 𝑅/2 𝑐 𝑃/2 𝑐 𝑌/2 + 𝑠 𝑅/2 𝑠 𝑃/2 𝑠 𝑌/2
𝑠 𝑅/2 𝑐 𝑃/2 𝑐 𝑌/2 − 𝑐 𝑅/2 𝑠 𝑃/2 𝑠 𝑌/2
𝑐 𝑅/2 𝑠 𝑃/2 𝑐 𝑌/2 + 𝑠 𝑅/2 𝑐 𝑃/2 𝑠 𝑌/2
𝑐 𝑅/2 𝑐 𝑃/2 𝑠 𝑌/2 − 𝑠 𝑅/2 𝑠 𝑃/2 𝑐 𝑌/2
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations BODY-NEDEuler Angles from Quaternions
Quaternions → Euler Angles
𝜽𝒏𝒃 =𝑅𝑃𝑌
=
atan2 2 𝑞2𝑞3 + 𝑞1𝑞0 , 1 − 2 𝑞12 + 𝑞2
2
asin 2 𝑞0𝑞2 − 𝑞3𝑞1
atan2 2 𝑞0𝑞3 + 𝑞1𝑞2 , 1 − 2 𝑞22 + 𝑞3
2
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations ECEF-NEDLat-Lon Transformations
ሶ𝒑 Τ𝑏 𝑒𝑒 = 𝑹𝑛
𝑒 𝜽𝑒𝑛 𝑹𝑏𝑛 𝜽𝑛𝑏 𝒗 Τ𝑏 𝑛
𝑏 = 𝑹𝑧,𝜆𝑹𝑦,−𝜑−𝜋2
ሶ𝒑 Τ𝑏 𝑛𝑛
𝑏
𝑛
𝑒
𝑥𝑒
𝑦𝑒
𝑧𝑒
Equator
𝑥𝑛𝑦𝑛
𝑧𝑛
𝜆
𝜑
𝜆 − 𝐿𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝜑 − 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒
𝑹𝑛𝑒 𝜽𝑒𝑛
𝑹𝑦,−𝜑−
𝜋2=
𝑐 −𝜑 −𝜋
20 𝑠 −𝜑 −
𝜋
20 1 0
−𝑠 −𝜑 −𝜋
20 𝑐 −𝜑 −
𝜋
2
𝑹𝑧,𝑌 =𝑐𝜆 −𝑠𝜆 0𝑠𝜆 𝑐𝜆 00 0 1
NED → ECEF
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations ECEF-NEDLat-Lon Transformations
NED → ECEF
ሶ𝒑 Τ𝑏 𝑒𝑒 = 𝑹𝑛
𝑒 𝜽𝑒𝑛 ሶ𝒑 Τ𝑏 𝑛𝑛 =
−𝑐𝜆𝑠𝜑 −𝑠𝜆 −𝑐𝜆𝑐𝜑−𝑠𝜆𝑠𝜑 𝑐𝜆 −𝑠𝜆𝑐𝜑𝑐𝜑 0 −𝑠𝜑
ሶ𝑥𝑛ሶ𝑦𝑛ሶ𝑧𝑛
𝑹𝑛𝑒 𝜽𝑒𝑛
North Velocity
East Velocity
Down Velocity
NEDECEF
ECEF → NED
ሶ𝒑 Τ𝑏 𝑛𝑛 = 𝑹𝑒
𝑛 𝜽𝑒𝑛 ሶ𝒑 Τ𝑏 𝑒𝑒 = 𝑹𝑛
𝑒𝑇 𝜽𝑒𝑛 ሶ𝒑 Τ𝑏 𝑒𝑒 =
−𝑐𝜆𝑠𝜑 −𝑠𝜆𝑠𝜑 𝑐𝜑−𝑠𝜆 𝑐𝜆 0−𝑐𝜆𝑐𝜑 −𝑠𝜆𝑐𝜑 −𝑠𝜑
ሶ𝑥𝑒ሶ𝑦𝑒ሶ𝑧𝑒
𝑹𝑒𝑛 𝜽𝑒𝑛
ECEFNED
ሶ𝒑 Τ𝑏 𝑛𝑛
ሶ𝒑 Τ𝑏 𝑒𝑒
Speed & Heading
Speed = ሶ𝑥𝑛2 + ሶ𝑦𝑛
2
Heading = tan−1𝑦𝑛𝑥𝑛
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations ECEF-NEDGeodetic Latitude vs Geocentric Latitude
Raw GNSS (GPS, GLONASS and Gallileo) output is given in Cartesian ECEF frame.𝒑 Τ𝑏 𝑒𝒆
It is presented to users in terms of longitude , latitude and height relative to WGS-84 ellipsoid.𝜆 𝜑 ℎ
𝜑 − 𝐺𝑒𝑜𝑑𝑒𝑡𝑖𝑐 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒𝜓 − 𝐺𝑒𝑜𝑐𝑒𝑛𝑡𝑟𝑖𝑐 𝐿𝑎𝑡𝑖𝑡𝑢𝑑𝑒
𝜓 𝜑 = tan−1 1 − 𝑒2 tan𝜑
𝑎 = 6378137.0𝑚 𝑠𝑒𝑚𝑖 − 𝑚𝑎𝑗𝑜𝑟 𝑎𝑥𝑖𝑠
WGS-84 parameters:
𝑏 = 6356752.3142𝑚 𝑠𝑒𝑚𝑖 − 𝑚𝑖𝑛𝑜𝑟 𝑎𝑥𝑖𝑠
𝑒 =𝑎2−𝑏2
𝑎2𝑒𝑐𝑐𝑒𝑛𝑡𝑟𝑖𝑐𝑖𝑡𝑦
North Pole
𝜑𝜓
ℎ
Tangent
Normal
Ellipsoid
𝑎
𝑏 𝑁
𝑓 = 𝑎 − 𝑏 /𝑎 "flattening" 𝑝𝑎𝑟𝑎𝑚𝑒𝑡𝑒𝑟
𝑒2 = 2𝑓 − 𝑓2
Geodetic Latitude vs Geocentric Latitude
𝑒 , =𝑎2 − 𝑏2
𝑏2
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations ECEF-NEDHeight Datums
1. Mean Sea Level (MSL) Datum (height relative to geoid)2. GPS height (height above WGS-84 ellipsoid)
ℎ
Earth’s surface
Geoid
WGS-84 Ellipsoid
𝐻
𝑀
𝜀𝑑
ℎ − 𝑒𝑙𝑙𝑖𝑝𝑠𝑜𝑖𝑑𝑎𝑙 ℎ𝑒𝑖𝑔ℎ𝑡 𝑔𝑒𝑜𝑑𝑒𝑡𝑖𝑐
𝐻 − 𝑜𝑟𝑡ℎ𝑜𝑚𝑒𝑡𝑟𝑖𝑐 ℎ𝑒𝑖𝑔ℎ𝑡 𝑀𝑆𝐿
𝑀 − 𝑔𝑒𝑜𝑖𝑑 𝑠𝑒𝑝𝑎𝑟𝑎𝑡𝑖𝑜𝑛 𝑢𝑛𝑑𝑢𝑙𝑎𝑡𝑖𝑜𝑛 𝑀 ≤ 100𝑚
ℎ ≈ 𝐻 +𝑀
𝜀𝑑 − 𝑑𝑒𝑓𝑙𝑒𝑐𝑡𝑖𝑜𝑛 𝑜𝑓 𝑡ℎ𝑒 𝑣𝑒𝑟𝑡𝑖𝑐𝑎𝑙 ≈ 0
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations ECEF-NEDECEF Coordinates from Lat-Lon
Lat-Lon → ECEF Coordinates
𝒑 Τ𝑏 𝑒𝒆 =
𝑥𝑒𝑦𝑒𝑧𝑒
=
𝑁 + ℎ cos𝜑 cos 𝜆
𝑁 + ℎ cos𝜑 sin 𝜆
𝑏2
𝑎2𝑁 + ℎ sin 𝜆
𝑁 =𝑎2
𝑎2cos2𝜑 + 𝑏2sin2𝜑𝑃𝑟𝑖𝑚𝑒 𝑉𝑒𝑟𝑡𝑖𝑐𝑎𝑙 𝑜𝑓 𝐶𝑢𝑟𝑣𝑎𝑡𝑢𝑟𝑒 (𝑚)
𝑁
Mobile & Marine Robotics Research Centre
University of Limerick
KinematicsTransformations ECEF-NEDLat-Lon from ECEF Coordinates
ECEF Coordinates → Lat-Lon
𝜆 = tan−1𝑦𝑒𝑥𝑒
𝑝 = 𝑥𝑒2 + 𝑦𝑒
2
𝜃 = tan−1𝑧𝑒𝑎
𝑝𝑏
𝜑 = tan−1𝑧𝑒 + 𝑒 ,2𝑏sin3𝜃
𝑝 − 𝑒2𝑎cos3𝜃
ℎ =𝑝
cos𝜑− 𝑁