Symbolic tools¶

The symbolic module provides tools for the exact analytic representation of trajectories and sensor data. It is optional and can be installed with the [symbolic] extras argument. It depends on the SymPy library and provides a subset of the numerical algorithms in the geometry and sensors modules.

Remarkably, it is possible to derive exact expressions for angular rate and specific force (non-gravitational acceleration) for arbitrarily complex trajectories that are composed from basic primitives. The same is true for useful intermediate values like coning, sculling, and scrolling values, derivatives and jacobian matrices.

The data can be used as a validation tool or as a direct simulation tool. Ideal gyro, accelerometer, IMU, or pose data can be input to sensor simulations in the sensors module or it can be used to validate algorithms in user applications. In both cases, it is advantageous to have an error-free analytic representation with which to compare measured or numerical equivalents.

Classes¶

The module provides a base class Trajectory and several trajectories that are used for internal validation:

Users can customize their own trajectories by passing the position, attitude (orientation), and gravity equations as functions of time in SymPy vector format to the initializer of the base class.

A standard set of parameters can then be returned in symbolic form or sampled numerically.

Advanced intermediates (such as generalized velocity or sculling and its time derivatives) can also be made available. See the complete API for further details.

Composing trajectories¶

Arbitrarily complex trajectories can be developed by composing multiple simpler elements using the Trajectory.compose function. An example, useful for validating the compensation of Earth's rotation in inertial navigation, is shown below.

Theoretical background¶

The rotation matrix representation of attitude, \(\mathbf{R}\), is a member of the \(SO(3)\) Lie group and is related to the rotation vector, \(\bm{\phi}\), a member of the associated \(\mathfrak{so}\)(3) Lie algebra. The two are related via the matrix exponential and logarithm which have closed forms. For background on Lie theory see References [16] & [18].

\[ \displaystyle \begin{aligned} \mathbf{R} &= exp([ \bm{\phi} \times ]) \\ &= \mathbf{I} + f_{1}[\bm{\phi} \times] + f_{2}[\bm{\phi} \times]^{2} \\ \end{aligned} \]

Where \([\bm{\phi}\times]\) is the anti-symmetric or cross-product matrix and \(f_{n}\) are scalars that are functions of the magnitude of the rotation. We use the \(f\) notation numbering from Savage, Reference [01]. The time derivative of the two representations are:

\[ \begin{aligned} \dot{\mathbf{R}} &= \mathbf{R}[\bm{\omega} \times] \\ \dot{\bm{\phi}} &= \left(\mathbf{I} + \frac{1}{2}[\bm{\phi} \times] + f_{5}[\bm{\phi} \times]^{2}\right)\bm{\omega} \end{aligned} \]

Where \(\bm{\omega}\) is the relative angular rate of the two frames as measured in the moving (body/sensor) frame. In inertial navigation, the rotation vector differential equation is known as the Bortz equation, from Reference [33]. Along with its inverse, it provides a method for relating rotation derivatives with angular rate as would be measured by a gyroscope.

\[ \bm{\omega} = \left(\mathbf{I} - f_{2}[\bm{\phi} \times] + f_{3}[\bm{\phi} \times]^{2}\right)\dot{\bm{\phi}} \]

Although they were derived by Bortz in a purely geometric fashion, it turns out that these equations are an example of a more general relation in Lie theory. The right Jacobian of the exponential relates derivatives in the Lie algebra representation to equivalent group derivatives. The inverse does the opposite.

\[ \begin{aligned} \bm{\omega} &= \mathbf{J}_{r}(\phi)\dot{\bm{\phi}} \\ \dot{\bm{\phi}} &= \mathbf{J}_{r}^{-1}(\phi)\bm{\omega} \end{aligned} \]

Since these equations are known in closed form for rotation and rigid body motions (\(SE(3)\) and related groups), we can implement them symbolically.

Coning¶

Coning is a classical test case for numerical attitude integration. It is defined as a rotation of a fixed magnitude about a rotation vector that oscillates out-of-phase around two axes simultaneously. The rotation vector inscribes a cone in the world frame, hence the name. Coning motion results in an angular rate that has a constant component in the third axis. Naive integration schemes will rectify this into a linear drift.

The original example from Bortz in Reference [33] is implemented in the ConingRotation class.

\[ \bm{\phi}_{coning}(t) = \begin{bmatrix} \theta \sin \omega t \\ \theta \cos \omega t \\ 0 \end{bmatrix} \]

Therefore

\[ \dot{\bm{\phi}}_{coning}(t) = \begin{bmatrix} \omega \theta \cos \omega t \\ -\omega \theta \sin \omega t \\ 0 \end{bmatrix} \]

Then applying the relations described above, we have

\[ \bm{\omega}_{coning}(t) = \begin{bmatrix} \omega\sin\theta \cos \omega t \\ -\omega \sin \theta \sin \omega t \\ \omega(1-\cos \theta) \end{bmatrix} \]

and

\[ \mathbf{R}_{coning}(t) = \begin{bmatrix} \cos \theta + (1-\cos\theta)\sin^{2}\omega t & \frac{1}{2}(1-\cos\theta)\sin 2\omega t & \sin\theta\cos\omega t \\ \frac{1}{2}(1-\cos\theta)\sin 2\omega t & \cos \theta + (1-\cos\theta)\cos^{2}\omega t & -\sin\theta\sin\omega t \\ -\sin\theta\cos\omega t & \sin\theta\sin\omega t & \cos\theta \end{bmatrix} \]

As a concrete example:

import numpy as np
import sympy

from inertialsim.plot import TimeSeries
from inertialsim.symbolic import ConingRotation
from inertialsim.time import span

# Pure symbolic coning motion.
coning = ConingRotation()
print("Angular rate (purely symbolic):")
print(sympy.simplify(coning.angular_rate()))

# Coning motion with 1.0 degree amplitude and 27Hz frequency.
# Sampled at 2kHz for 0.1 seconds.
dt = 0.0005
time = span(0, 0.1, dt)
coning = ConingRotation(amplitude=np.deg2rad(1.0), frequency=27.0)
print()
print("Angular rate (numeric coefficients):")
print(sympy.simplify(coning.angular_rate()))

# Sample the angular rate and plot
angular_rate = coning.angular_rate(time)

ar_plot = TimeSeries(title="Sampled Angular Rate", ylabel="(rad/s)")
ar_plot.line(time, angular_rate)

Angular rate (purely symbolic):
(ω*sin(θ)*cos(t*ω))*coordinates.i + (-ω*sin(θ)*sin(t*ω))*coordinates.j + (ω*(1 - cos(θ)))*coordinates.k

Angular rate (numeric coefficients):
(2.96073099994499*cos(169.646003293849*t))*coordinates.i + (-2.96073099994499*sin(169.646003293849*t))*coordinates.j + 0.0258379080005158*coordinates.k

png

General motion¶

The rotational developments above can be extended to general motion in six degrees of freedom using equivalent formulations with the \(SE(3)\) Lie group (and its variations incorporating velocity).

ZigZag motion¶

The ZigZagTrajectory trajectory provides motion in all axes of orientation and position. The attitude is described by independent oscillations in magnitude and frequency around each axis. The position is defined by a zigzag pattern in the x-y plane and oscillation in the z-axis. This approximates the motion of a holonomic vehicle, like a drone, translating and rotating in complex ways.

The trajectory is specified relative to a local navigation frame with standard gravity in the vertical axis.

\[ \bm{\phi}_{zigzag}(t) = \begin{bmatrix} \theta_{x} \sin \omega_{ax} t \\ \theta_{y} \cos \omega_{ay} t \\ \theta_{z} \sin \omega_{az} t \\ \end{bmatrix} \]

\[ \mathbf{p}_{zigzag}(t) = \begin{bmatrix*}[l] A_{x} \sin \omega_{px} t \\ A_{y} \sin \omega_{py} t \cos \omega_{py} t \\ A_{z} \cos \omega_{pz} t \\ \end{bmatrix*} \]

The analytic form of the specific force is too large to display in a useful way, so we just illustrate a fraction of it (the first 1000 of ~175000 characters).

from inertialsim.plot import Scatter
from inertialsim.symbolic import ZigZagTrajectory

# Pure symbolic motion.
zigzag = ZigZagTrajectory()
print("Specific force:")
sf = str(zigzag.specific_force())
print(sf[0:1000] + "...")

Specific force:
(θx*ωx*((1 - sin(sqrt(θx**2*sin(t*ωx)**2 + θy**2*cos(t*ωy)**2 + θz**2*sin(t*ωz)**2))/sqrt(θx**2*sin(t*ωx)**2 + θy**2*cos(t*ωy)**2 + θz**2*sin(t*ωz)**2))*(θy*θz*(-Ax*ωpx*cos(t*ωpx) + 0.5*θy*(-Az*ωpz*sin(t*ωpz) - 9.80665*t)*cos(t*ωy) + 0.5*θz*(Ay*ωpy*sin(t*ωpy)**2 - Ay*ωpy*cos(t*ωpy)**2)*sin(t*ωz) - (1 - sqrt(θx**2*sin(t*ωx)**2 + θy**2*cos(t*ωy)**2 + θz**2*sin(t*ωz)**2)*sin(sqrt(θx**2*sin(t*ωx)**2 + θy**2*cos(t*ωy)**2 + θz**2*sin(t*ωz)**2))/(2 - 2*cos(sqrt(θx**2*sin(t*ωx)**2 + θy**2*cos(t*ωy)**2 + θz**2*sin(t*ωz)**2))))*(θy*(-Ax*θy*ωpx*cos(t*ωpx)*cos(t*ωy) + θx*(-Ay*ωpy*sin(t*ωpy)**2 + Ay*ωpy*cos(t*ωpy)**2)*sin(t*ωx))*cos(t*ωy) + θz*(-Ax*θz*ωpx*sin(t*ωz)*cos(t*ωpx) - θx*(Az*ωpz*sin(t*ωpz) + 9.80665*t)*sin(t*ωx))*sin(t*ωz))/(θx**2*sin(t*ωx)**2 + θy**2*cos(t*ωy)**2 + θz**2*sin(t*ωz)**2))*sin(t*ωz)*cos(t*ωy) + θy*θz*(Ax*ωpx*cos(t*ωpx) - 0.5*θy*(-Az*ωpz*sin(t*ωpz) - 9.80665*t)*cos(t*ωy) - 0.5*θz*(Ay*ωpy*sin(t*ωpy)**2 - Ay*ωpy*cos(t*ωpy)**2)*sin(t*ωz) + (1 - sqrt(θx**2*sin(t*ωx)**2 + θy**2*co...

An illustrative example is given below:

# Zigzag motion profile.
# +/- 70 degree yaw sweep once every 50s (0.02Hz) with higher frequency, small
# amplitude roll and pitch motions.  Low frequency zigzag position in x, y axes
# with smaller oscillations in z-axis. Sampled at 100Hz for 100 seconds.
dt = 0.01
duration = 100.0
time = span(0, duration, dt)
zigzag = ZigZagTrajectory(
    attitude_amplitudes=np.deg2rad(np.array([2.0, 3.0, 70.0])),
    attitude_frequencies=np.array([0.25, 0.4, 0.02]),
    position_amplitudes=np.array([100, 75, 5]),
    position_frequencies=np.array([0.01, 0.02, 0.034]),
)

# Sample the position and plot
position = zigzag.position(time)
p_plot = Scatter(title="Sampled Position", xlabel="x (m)", ylabel="y (m)")
p_plot.line(position[:, 0], position[:, 1])

# Sample the specific force and plot
specific_force = zigzag.specific_force(time)
sf_plot = TimeSeries(title="Sampled Specific Force", ylabel="(m/s/s)")
sf_plot.line(time, specific_force)
sf_plot.legend(["X", "Y", "Z"], loc="lower right")

png

Earth rotation¶

To create more complex and realistic motion profiles, trajectories can be composed. As a final example, the symbolic module includes the EarthRotation trajectory. This models the rotation of Earth relative to inertial space.

\[ \bm{\phi}_{earth}(t) = \begin{bmatrix} \omega_{x} t \\ \omega_{y} t \\ \omega_{z} t \\ \end{bmatrix} \]

Where the EarthRate class can be used to calculate the Earth rate components in any relevant frame.

The zigzag trajectory, described above, is specified relative to a local fixed frame. If we compose it with Earth's rotation relative to inertial space, the resulting angular rate and specific force values will also then be calculated relative to inertial space, exactly as they are measured by real sensors. One application of this is to study the compensation of coriolis and centripetal effects in inertial navigation.

from inertialsim.geodesy.datums import WGS84
from inertialsim.symbolic import EarthRotation

# Earth rotation with 15 deg/h rate exaggerated 10x
earth = EarthRotation(angular_rate=[0, 0, 10 * WGS84.angular_velocity])

# The overall rotation, as the composition of both.
zigzag_earth = earth.compose(zigzag)

position_earth = zigzag.position(time)
position_inertial = zigzag_earth.position(time)
p_plot = Scatter(title="Sampled Position", xlabel="x (m)", ylabel="y (m)")
p_plot.line(position[:, 0], position[:, 1])
p_plot.line(position_inertial[:, 0], position_inertial[:, 1])
p_plot.legend(["Zigzag", "Zigzag w/ Earth Rotation"], loc="lower right")

png

Limitations¶

In practice the underlying symbolic solver, SymPy, can struggle with highly complex trajectories with many degrees of freedom.

While it is possible to specify a closed form (non-iterative) symbolic ellipsoidal gravity model it is not currently implemented in the symbolic module. Constant gravity and gravity variation with altitude are easily supported.

Similarly it is possible to specify closed form conversions to and from geodetic coordinates (latitude and longitude) but these are not currently implemented in the symbolic module. Topocentric and ECEF global frames can be supported subject to the gravity limitation described above.

Geodesy module

Exact, closed form equations for converting to geodetic coordinates and calculating gravity from an ellipsoidal Earth model are supported in the geodesy module. They are implemented numerically rather than symbolically.