IEKF_NavstateExample.cpp File Reference

InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update) More...

#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h>
#include <iostream>

Include dependency graph for IEKF_NavstateExample.cpp:

Go to the source code of this file.

Functions
Vector9	dynamics (const Vector6 &imu)
	Left-invariant dynamics for NavState. More...
Vector3	h_gps (const NavState &X, OptionalJacobian< 3, 9 > H={})
	GPS measurement model: returns position and its Jacobian. More...
int	main ()

Detailed Description

InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)

Date

April 25, 2025

Authors

Scott Baker, Matt Kielo, Frank Dellaert

Definition in file IEKF_NavstateExample.cpp.

Function Documentation

dynamics()

Vector9 dynamics

(

const Vector6 &

imu

)

Left-invariant dynamics for NavState.

Parameters

imu	6×1 vector [a; ω]: linear acceleration and angular velocity.

Returns

9×1 tangent: [ω; 0₃; a].

Definition at line 35 of file IEKF_NavstateExample.cpp.

h_gps()

Vector3 h_gps	(	const NavState &	X,
		OptionalJacobian< 3, 9 >	H = {}
	)

GPS measurement model: returns position and its Jacobian.

Parameters

X	Current state.
H	Optional 3×9 Jacobian w.r.t. X.

Returns

3×1 position vector.

Definition at line 49 of file IEKF_NavstateExample.cpp.

main()

int main

(

)

Definition at line 53 of file IEKF_NavstateExample.cpp.