NavState

The NavState class represents attitude, position, and velocity as a 9D manifold and also implements the matrix Lie group commonly denoted $SE_2(3)$ in the inertial-navigation literature. It is the core state type behind IMU preintegration and several navigation factors in GTSAM.

import numpy as np
from gtsam import NavState, Rot3, Point3

`NavState` as a manifold¶

A NavState is a tuple $(R, p, v)$ where:

$R \in SO(3)$ is attitude.
$p \in \mathbb{R}^3$ is position in the navigation/world frame.
$v \in \mathbb{R}^3$ is velocity in the navigation/world frame.

In GTSAM, local increments for this manifold are ordered as rotation, then position, then velocity.

# Identity state
X_identity = NavState()
print(f"Identity state:\n{X_identity}\n")

# A custom navigation state
R = Rot3.Yaw(np.pi / 6)
p = Point3(10, 20, 30)
v = np.array([1, 2, 3])
X1 = NavState(R, p, v)
print(f"Custom state X1:\n{X1}")

Identity state:
R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
p: 0 0 0
v: 0 0 0


Custom state X1:
R: [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]
p: 10 20 30
v: 1 2 3

The state components are accessible through attitude(), position(), and velocity(). You can also get pose() (rotation + position only) and bodyVelocity() (velocity expressed in the body frame).

print(f"Attitude:\n{X1.attitude()}\n")
print(f"Position: {X1.position()}")
print(f"Velocity: {X1.velocity()}")
print(f"Body-frame velocity: {np.round(X1.bodyVelocity(), 4)}")
print(f"Pose:\n{X1.pose()}")

Attitude:
R: [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]


Position: [10. 20. 30.]
Velocity: [1. 2. 3.]
Body-frame velocity: [1.866  1.2321 3.    ]
Pose:
R: [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]
t: 10 20 30

Connection to $SE_2(3)$ and ordering conventions¶

NavState follows the same Lie-group structure used in the $SE_2(3)$ literature, but with a different storage/order convention for compatibility with legacy GTSAM code.

This notebook / NavState convention (R, p, v):
$X = \begin{bmatrix} R & p & v \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}$
(1)
Common literature convention (often Barrau et al.):
$X_{lit} = \begin{bmatrix} R & v & p \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}$
(2)

Likewise, the NavState tangent vector uses order [dR, dP, dV] (rotation, position, velocity). Many papers use [dR, dV, dP] to match their matrix ordering.

For comparison: in Gal3, we do follow the literature ordering conventions used there.

T = X1.matrix()
print("NavState matrix (R, p, v):")
print(np.round(T, 4))

xi_rpv = np.array([0.1, -0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6])  # [dR, dP, dV]
xi_rvp = np.hstack([xi_rpv[0:3], xi_rpv[6:9], xi_rpv[3:6]])          # [dR, dV, dP]
print("xi in NavState order [dR, dP, dV]:", xi_rpv)
print("same increments in literature-like [dR, dV, dP] order:", xi_rvp)

NavState matrix (R, p, v):
[[ 0.866 -0.5    0.    10.     1.   ]
 [ 0.5    0.866  0.    20.     2.   ]
 [ 0.     0.     1.    30.     3.   ]
 [ 0.     0.     0.     1.     0.   ]
 [ 0.     0.     0.     0.     1.   ]]
xi in NavState order [dR, dP, dV]: [ 0.1 -0.2  0.3  1.   2.   3.   0.4  0.5  0.6]
same increments in literature-like [dR, dV, dP] order: [ 0.1 -0.2  0.3  0.4  0.5  0.6  1.   2.   3. ]

Group operations¶

As a Lie group, NavState supports composition and inversion. The composition matches matrix multiplication of the 5x5 representation.

X2 = NavState(Rot3.Yaw(np.pi / 12), Point3(5, 5, 5), np.array([4, 2, 5]))

X1_inv = X1.inverse()
print(f"X1.inverse():\n{X1_inv}\n")

X_comp = X1 * X2
print(f"X1 * X2:\n{X_comp}\n")

lhs = X_comp.matrix()
rhs = X1.matrix() @ X2.matrix()
print("matrix(X1 * X2) == matrix(X1) @ matrix(X2):", np.allclose(lhs, rhs))

X1.inverse():
R: [
	0.866025, 0.5, 0;
	-0.5, 0.866025, 0;
	0, 0, 1
]
p: -18.6603 -12.3205      -30
v: -1.86603 -1.23205       -3


X1 * X2:
R: [
	0.707107, -0.707107, 0;
	0.707107, 0.707107, 0;
	0, 0, 1
]
p: 11.8301 26.8301      35
v:  3.4641 5.73205       8


matrix(X1 * X2) == matrix(X1) @ matrix(X2): True

Lie algebra and manifold operations¶

The 9D tangent coordinates are ordered as [R_x, R_y, R_z, P_x, P_y, P_z, V_x, V_y, V_z].

Expmap and Logmap convert between tangent vectors and NavState group elements. retract/localCoordinates are the manifold operations used in optimization.

xi = np.array([0.1, 0.2, -0.1, 0.4, 0.5, 0.6, -0.2, 0.3, 0.1])  # [dR, dP, dV]

X_exp = NavState.Expmap(xi)
print(f"Expmap(xi):\n{X_exp}\n")

xi_log = NavState.Logmap(X_exp)
print("Logmap(Expmap(xi)) =", np.round(xi_log, 6))

Expmap(xi):
R: [
	0.975125, 0.108953, 0.193031;
	-0.0890529, 0.99005, -0.108953;
	-0.202981, 0.0890529, 0.975125
]
p: 0.481917 0.447923 0.577763
v: -0.172632  0.302981   0.13333


Logmap(Expmap(xi)) = [ 0.1  0.2 -0.1  0.4  0.5  0.6 -0.2  0.3  0.1]

Xp = NavState(Rot3.Yaw(np.pi / 4), Point3(15, 30, 50), np.array([-2, 0, 0]))
Xq = X1

delta = Xp.localCoordinates(Xq)
print("localCoordinates from Xp to Xq =", np.round(delta, 6))

Xq_retracted = Xp.retract(delta)
print(f"Xp.retract(delta):\n{Xq_retracted}")

localCoordinates from Xp to Xq = [  0.         0.        -0.261799 -10.606602  -3.535534 -20.
   3.535534  -0.707107   3.      ]
Xp.retract(delta):
R: [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]
p: 10 20 30
v: 1 2 3

NavState as a manifold¶

Connection to SE2(3)SE_2(3)SE2​(3) and ordering conventions¶

Group operations¶

Lie algebra and manifold operations¶

`NavState` as a manifold¶

Connection to $SE_2(3)$ and ordering conventions¶