ErrorEquations.hpp File Reference

Error Equations for the ECEF frame. More...

Go to the source code of this file.

Functions
template<typename Derived>
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	NAV::e_F_df_df (const Eigen::MatrixBase< Derived > &beta_a)
	Calculates the matrix 𝐅_𝛿f'_𝛿f.
template<typename T>
Eigen::Matrix3< T >	NAV::e_F_dpsi_dpsi (const T &omega_ie)
	Calculates the matrix 𝐅_𝜓'_𝜓
template<typename Derived>
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	NAV::e_F_dpsi_dw (const Eigen::MatrixBase< Derived > &e_Dcm_b)
	Calculates the matrix 𝐅_𝜓'_𝛿ω
template<typename Derived1, typename Derived2>
Eigen::Matrix< typename Derived2::Scalar, 4, 3 >	NAV::e_F_dq_dGyroBias (const Eigen::QuaternionBase< Derived1 > &b_quat_p, const Eigen::QuaternionBase< Derived2 > &e_quat_b)
	Calculate the linearized attitude quaternion differential equation partially derived for the angular rate bias.
template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>
Eigen::Matrix4< typename Derived5::Scalar >	NAV::e_F_dq_dGyroMisalignment (const Eigen::MatrixBase< Derived1 > &p_gyroScale, const Eigen::QuaternionBase< Derived2 > &p_quatGyro_ps, const Eigen::MatrixBase< Derived3 > &ps_omega_ip, const Eigen::QuaternionBase< Derived4 > &b_quat_p, const Eigen::QuaternionBase< Derived5 > &e_quat_b)
	Calculate the linearized attitude quaternion differential equation partially derived for the gyroscope misalignment.
template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
Eigen::Matrix< typename Derived4::Scalar, 4, 3 >	NAV::e_F_dq_dGyroScale (const Eigen::QuaternionBase< Derived1 > &p_quatGyro_ps, const Eigen::MatrixBase< Derived2 > &ps_omega_ip, const Eigen::QuaternionBase< Derived3 > &b_quat_p, const Eigen::QuaternionBase< Derived4 > &e_quat_b)
	Calculate the linearized attitude quaternion differential equation partially derived for the angular rate scale factor.
template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6>
Eigen::Matrix4< typename Derived6::Scalar >	NAV::e_F_dq_dq (const Eigen::MatrixBase< Derived1 > &p_gyroBias, const Eigen::MatrixBase< Derived2 > &p_gyroScale, const Eigen::QuaternionBase< Derived3 > &p_quatGyro_ps, const Eigen::MatrixBase< Derived4 > &ps_omega_ip, const Eigen::QuaternionBase< Derived5 > &b_quat_p, const Eigen::QuaternionBase< Derived6 > &e_quat_b, double omega_ie)
	Calculate the linearized attitude quaternion differential equation partially derived for the attitude quaternion.
template<typename T>
Eigen::Matrix3< T >	NAV::e_F_dr_dv ()
	Calculates the matrix 𝐅_𝛿r'_𝛿v.
template<typename Derived1, typename Derived2>
Eigen::Matrix< typename Derived2::Scalar, 3, 3 >	NAV::e_F_dv_dAccelBias (const Eigen::QuaternionBase< Derived1 > &b_quat_p, const Eigen::QuaternionBase< Derived2 > &e_quat_b)
	Calculates the matrix 𝐅_𝜓'_𝛿f_p.
template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>
Eigen::Matrix< typename Derived5::Scalar, 3, 4 >	NAV::e_F_dv_dAccelMisalignment (const Eigen::MatrixBase< Derived1 > &p_accelScale, const Eigen::QuaternionBase< Derived2 > &p_quatAccel_ps, const Eigen::MatrixBase< Derived3 > &ps_f_ip, const Eigen::QuaternionBase< Derived4 > &b_quat_p, const Eigen::QuaternionBase< Derived5 > &e_quat_b)
	Calculate the linearized velocity differential equation partially derived for the accelerometer misalignment.
template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
Eigen::Matrix< typename Derived4::Scalar, 3, 3 >	NAV::e_F_dv_dAccelScale (const Eigen::QuaternionBase< Derived1 > &p_quatAccel_ps, const Eigen::MatrixBase< Derived2 > &ps_f_ip, const Eigen::QuaternionBase< Derived3 > &b_quat_p, const Eigen::QuaternionBase< Derived4 > &e_quat_b)
	Calculate the linearized velocity differential equation partially derived for the acceleration bias.
template<typename Derived>
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	NAV::e_F_dv_df_b (const Eigen::MatrixBase< Derived > &e_Dcm_b)
	Calculates the matrix 𝐅_𝜓'_𝛿f_b.
template<typename Derived>
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	NAV::e_F_dv_dpsi (const Eigen::MatrixBase< Derived > &e_force_ib)
	Calculates the matrix 𝐅_𝛿v'_𝜓
template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6>
Eigen::Matrix< typename Derived6::Scalar, 3, 4 >	NAV::e_F_dv_dq (const Eigen::MatrixBase< Derived1 > &p_accelBias, const Eigen::MatrixBase< Derived2 > &p_accelScale, const Eigen::QuaternionBase< Derived3 > &p_quatAccel_ps, const Eigen::MatrixBase< Derived4 > &ps_f_ip, const Eigen::QuaternionBase< Derived5 > &b_quat_p, const Eigen::QuaternionBase< Derived6 > &e_quat_b)
	Calculate the linearized velocity differential equation partially derived for the attitude.
template<typename Derived1, typename Derived2, typename T>
Eigen::Matrix< typename Derived1::Scalar, 3, 3 >	NAV::e_F_dv_dr (const Eigen::MatrixBase< Derived1 > &e_position, const Eigen::MatrixBase< Derived2 > &e_gravitation, const T &r_eS_e, const Eigen::Vector3d &e_omega_ie)
	Calculates the matrix 𝐅_𝛿v'_𝛿r.
template<typename T>
Eigen::Matrix3< T >	NAV::e_F_dv_dv (double omega_ie)
	Calculates the matrix 𝐅_𝛿v'_𝛿v.
template<typename Derived>
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	NAV::e_F_dw_dw (const Eigen::MatrixBase< Derived > &beta_omega)
	Calculates the matrix 𝐅_𝛿ω'_𝛿ω
template<typename KeyType, typename T>
KeyedMatrixX< T, KeyType, KeyType >	NAV::e_systemMatrix_F (const Eigen::Vector3d &ps_f_ip, const Eigen::Vector3d &ps_omega_ip, const Eigen::Quaterniond &b_quat_p, const Eigen::Vector3< T > &e_gravitation, const T &r_eS_e, std::optional< double > tau_bad, std::optional< double > tau_bgd, const Eigen::Vector3< T > &e_position, const Eigen::Quaternion< T > &e_quat_b, const Eigen::Vector3< T > &p_accelBias, const Eigen::Vector3< T > &p_gyroBias, const Eigen::Vector3< T > &p_accelScale, const Eigen::Vector3< T > &p_gyroScale, const Eigen::Quaternion< T > &p_quatAccel_ps, const Eigen::Quaternion< T > &p_quatGyro_ps, const PosVelAttDerivativeConstants &config, bool accelBiases, bool gyroBiases, bool accelScaleFactor, bool gyroScaleFactor, bool accelMisalignment, bool gyroMisalignment)
	Calculate the linearized system matrix in Earth frame coordinates.

Detailed Description

Error Equations for the ECEF frame.

Author

T. Topp (topp@.nosp@m.ins..nosp@m.uni-s.nosp@m.tutt.nosp@m.gart..nosp@m.de)

Date

2022-06-12

Function Documentation

e_F_df_df()

template<typename Derived>

Eigen::Matrix< typename Derived::Scalar, 3, 3 > NAV::e_F_df_df ( const Eigen::MatrixBase< Derived > & beta_a )

nodiscard

Calculates the matrix 𝐅_𝛿f'_𝛿f.

Parameters

[in]

beta_a

Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length)

Returns

3x3 matrix in [1 / s]

Note

See T. Hobiger (2021) Inertialnavigation V06 - equation (6.3)

e_F_dpsi_dpsi()

template<typename T>

Eigen::Matrix3< T > NAV::e_F_dpsi_dpsi ( const T & omega_ie )

nodiscard

Calculates the matrix 𝐅_𝜓'_𝜓

Parameters

[in]

omega_ie

Angular velocity of the Earth in [rad/s]

Returns

3x3 matrix in [rad / s]

Note

See [17] Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_11

e_F_dpsi_dw()

template<typename Derived>

Eigen::Matrix< typename Derived::Scalar, 3, 3 > NAV::e_F_dpsi_dw ( const Eigen::MatrixBase< Derived > & e_Dcm_b )

nodiscard

Calculates the matrix 𝐅_𝜓'_𝛿ω

Parameters

[in]

e_Dcm_b

DCM from body to Earth frame

Returns

3x3 matrix in [-]

Note

See [17] Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_15

e_F_dq_dGyroBias()

template<typename Derived1, typename Derived2>

Eigen::Matrix< typename Derived2::Scalar, 4, 3 > NAV::e_F_dq_dGyroBias ( const Eigen::QuaternionBase< Derived1 > & b_quat_p, const Eigen::QuaternionBase< Derived2 > & e_quat_b )

nodiscard

Calculate the linearized attitude quaternion differential equation partially derived for the angular rate bias.

Parameters

b_quat_p	Quaternion from IMU platform frame to body frame
e_quat_b	Quaternion from body to Earth frame

e_F_dq_dGyroMisalignment()

template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>

Eigen::Matrix4< typename Derived5::Scalar > NAV::e_F_dq_dGyroMisalignment ( const Eigen::MatrixBase< Derived1 > & p_gyroScale, const Eigen::QuaternionBase< Derived2 > & p_quatGyro_ps, const Eigen::MatrixBase< Derived3 > & ps_omega_ip, const Eigen::QuaternionBase< Derived4 > & b_quat_p, const Eigen::QuaternionBase< Derived5 > & e_quat_b )

nodiscard

Calculate the linearized attitude quaternion differential equation partially derived for the gyroscope misalignment.

Parameters

p_gyroScale	Angular rate scale factor in platform frame coordinates [rad/s]
p_quatGyro_ps	Gyroscope misalignment
ps_omega_ip	Gyroscope measurement [m/s^2]
b_quat_p	Quaternion from IMU platform frame to body frame
e_quat_b	Quaternion from body to Earth frame

e_F_dq_dGyroScale()

template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>

Eigen::Matrix< typename Derived4::Scalar, 4, 3 > NAV::e_F_dq_dGyroScale ( const Eigen::QuaternionBase< Derived1 > & p_quatGyro_ps, const Eigen::MatrixBase< Derived2 > & ps_omega_ip, const Eigen::QuaternionBase< Derived3 > & b_quat_p, const Eigen::QuaternionBase< Derived4 > & e_quat_b )

nodiscard

Calculate the linearized attitude quaternion differential equation partially derived for the angular rate scale factor.

Parameters

p_quatGyro_ps	Gyroscope misalignment
ps_omega_ip	Gyroscope measurement [m/s^2]
b_quat_p	Quaternion from IMU platform frame to body frame
e_quat_b	Quaternion from body to Earth frame

e_F_dq_dq()

template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6>

Eigen::Matrix4< typename Derived6::Scalar > NAV::e_F_dq_dq ( const Eigen::MatrixBase< Derived1 > & p_gyroBias, const Eigen::MatrixBase< Derived2 > & p_gyroScale, const Eigen::QuaternionBase< Derived3 > & p_quatGyro_ps, const Eigen::MatrixBase< Derived4 > & ps_omega_ip, const Eigen::QuaternionBase< Derived5 > & b_quat_p, const Eigen::QuaternionBase< Derived6 > & e_quat_b, double omega_ie )

nodiscard

Calculate the linearized attitude quaternion differential equation partially derived for the attitude quaternion.

Parameters

p_gyroBias	Angular rate bias in platform frame coordinates [rad/s]
p_gyroScale	Angular rate scale factor in platform frame coordinates [rad/s]
p_quatGyro_ps	Gyroscope misalignment
ps_omega_ip	Gyroscope measurement [m/s^2]
b_quat_p	Quaternion from IMU platform frame to body frame
e_quat_b	Quaternion from body to Earth frame
omega_ie	Earth rotation rate [rad/s]

e_F_dr_dv()

template<typename T>

Eigen::Matrix3< T > NAV::e_F_dr_dv ( )

nodiscard

Calculates the matrix 𝐅_𝛿r'_𝛿v.

Returns

3x3 matrix in [-]

Note

See [17] Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_32

e_F_dv_dAccelBias()

template<typename Derived1, typename Derived2>

Eigen::Matrix< typename Derived2::Scalar, 3, 3 > NAV::e_F_dv_dAccelBias ( const Eigen::QuaternionBase< Derived1 > & b_quat_p, const Eigen::QuaternionBase< Derived2 > & e_quat_b )

nodiscard

Calculates the matrix 𝐅_𝜓'_𝛿f_p.

Parameters

[in]	b_quat_p	Quaternion from IMU platform frame to body frame
[in]	e_quat_b	Quaternion from body to Earth frame

Returns

3x3 matrix in [-]

e_F_dv_dAccelMisalignment()

template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>

Eigen::Matrix< typename Derived5::Scalar, 3, 4 > NAV::e_F_dv_dAccelMisalignment ( const Eigen::MatrixBase< Derived1 > & p_accelScale, const Eigen::QuaternionBase< Derived2 > & p_quatAccel_ps, const Eigen::MatrixBase< Derived3 > & ps_f_ip, const Eigen::QuaternionBase< Derived4 > & b_quat_p, const Eigen::QuaternionBase< Derived5 > & e_quat_b )

nodiscard

Calculate the linearized velocity differential equation partially derived for the accelerometer misalignment.

Parameters

p_accelScale	Acceleration scale factor in platform frame coordinates [m/s^2]
p_quatAccel_ps	Accelerometer misalignment
ps_f_ip	Accelerometer measurement [m/s^2]
b_quat_p	Quaternion from IMU platform frame to body frame
e_quat_b	Quaternion from body to Earth frame

e_F_dv_dAccelScale()

template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>

Eigen::Matrix< typename Derived4::Scalar, 3, 3 > NAV::e_F_dv_dAccelScale ( const Eigen::QuaternionBase< Derived1 > & p_quatAccel_ps, const Eigen::MatrixBase< Derived2 > & ps_f_ip, const Eigen::QuaternionBase< Derived3 > & b_quat_p, const Eigen::QuaternionBase< Derived4 > & e_quat_b )

nodiscard

Calculate the linearized velocity differential equation partially derived for the acceleration bias.

Parameters

p_quatAccel_ps	Accelerometer misalignment
ps_f_ip	Accelerometer measurement [m/s^2]
b_quat_p	Quaternion from IMU platform frame to body frame
e_quat_b	Quaternion from body to Earth frame

e_F_dv_df_b()

template<typename Derived>

Eigen::Matrix< typename Derived::Scalar, 3, 3 > NAV::e_F_dv_df_b ( const Eigen::MatrixBase< Derived > & e_Dcm_b )

nodiscard

Calculates the matrix 𝐅_𝜓'_𝛿f_b.

Parameters

[in]

e_Dcm_b

DCM from body to Earth frame

Returns

3x3 matrix in [-]

Note

See [17] Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_24

e_F_dv_dpsi()

template<typename Derived>

Eigen::Matrix< typename Derived::Scalar, 3, 3 > NAV::e_F_dv_dpsi ( const Eigen::MatrixBase< Derived > & e_force_ib )

nodiscard

Calculates the matrix 𝐅_𝛿v'_𝜓

Parameters

[in]

e_force_ib

Specific force of the body with respect to inertial frame in [m / s^2], resolved in Earth frame coordinates

Returns

3x3 matrix in [m / s^2]

Note

See [17] Groves, ch. 14.2.3, eq. 14.49, p. 584 - 𝐅_21

e_F_dv_dq()

template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6>

Eigen::Matrix< typename Derived6::Scalar, 3, 4 > NAV::e_F_dv_dq	(	const Eigen::MatrixBase< Derived1 > &	p_accelBias,
		const Eigen::MatrixBase< Derived2 > &	p_accelScale,
		const Eigen::QuaternionBase< Derived3 > &	p_quatAccel_ps,
		const Eigen::MatrixBase< Derived4 > &	ps_f_ip,
		const Eigen::QuaternionBase< Derived5 > &	b_quat_p,
		const Eigen::QuaternionBase< Derived6 > &	e_quat_b )

Calculate the linearized velocity differential equation partially derived for the attitude.

Parameters

p_accelBias	Acceleration bias in platform frame coordinates [m/s^2]
p_accelScale	Acceleration scale factor in platform frame coordinates [m/s^2]
p_quatAccel_ps	Accelerometer misalignment
ps_f_ip	Accelerometer measurement [m/s^2]
b_quat_p	Quaternion from IMU platform frame to body frame
e_quat_b	Quaternion from body to Earth frame

e_F_dv_dr()

template<typename Derived1, typename Derived2, typename T>

Eigen::Matrix< typename Derived1::Scalar, 3, 3 > NAV::e_F_dv_dr ( const Eigen::MatrixBase< Derived1 > & e_position, const Eigen::MatrixBase< Derived2 > & e_gravitation, const T & r_eS_e, const Eigen::Vector3d & e_omega_ie )

nodiscard

Calculates the matrix 𝐅_𝛿v'_𝛿r.

Parameters

[in]	e_position	Position in ECEF coordinates in [m]
[in]	e_gravitation	Gravitational acceleration in [m/s^2]
[in]	r_eS_e	Geocentric radius. The distance of a point on the Earth's surface from the center of the Earth in [m]
[in]	e_omega_ie	Angular velocity of Earth with respect to inertial system, represented in e-sys in [rad/s]

Returns

3x3 matrix in [1 / s^2]

Note

See [17] Groves, ch. 14.2.3, eq. 14.49, p. 584 - 𝐅_23

e_F_dv_dv()

template<typename T>

Eigen::Matrix3< T > NAV::e_F_dv_dv ( double omega_ie )

nodiscard

Calculates the matrix 𝐅_𝛿v'_𝛿v.

Parameters

[in]

omega_ie

omega_ie Angular velocity of the Earth in [rad/s]

Returns

3x3 matrix in [1 / s]

Note

See [17] Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_22

e_F_dw_dw()

template<typename Derived>

Eigen::Matrix< typename Derived::Scalar, 3, 3 > NAV::e_F_dw_dw ( const Eigen::MatrixBase< Derived > & beta_omega )

nodiscard

Calculates the matrix 𝐅_𝛿ω'_𝛿ω

Parameters

[in]

beta_omega

Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length)

Returns

3x3 matrix in [1 / s]

Note

See T. Hobiger (2021) Inertialnavigation V06 - equation (6.3)

e_systemMatrix_F()

template<typename KeyType, typename T>

KeyedMatrixX< T, KeyType, KeyType > NAV::e_systemMatrix_F ( const Eigen::Vector3d & ps_f_ip, const Eigen::Vector3d & ps_omega_ip, const Eigen::Quaterniond & b_quat_p, const Eigen::Vector3< T > & e_gravitation, const T & r_eS_e, std::optional< double > tau_bad, std::optional< double > tau_bgd, const Eigen::Vector3< T > & e_position, const Eigen::Quaternion< T > & e_quat_b, const Eigen::Vector3< T > & p_accelBias, const Eigen::Vector3< T > & p_gyroBias, const Eigen::Vector3< T > & p_accelScale, const Eigen::Vector3< T > & p_gyroScale, const Eigen::Quaternion< T > & p_quatAccel_ps, const Eigen::Quaternion< T > & p_quatGyro_ps, const PosVelAttDerivativeConstants & config, bool accelBiases, bool gyroBiases, bool accelScaleFactor, bool gyroScaleFactor, bool accelMisalignment, bool gyroMisalignment )

nodiscard

Calculate the linearized system matrix in Earth frame coordinates.

Parameters

ps_f_ip	Accelerometer measurement [m/s^2]
ps_omega_ip	Gyroscope measurement [m/s^2]
b_quat_p	Quaternion from IMU platform frame to body frame
e_gravitation	Gravitation in [m/s^2] in ECEF coordinates
r_eS_e	Geocentric radius in [m]
tau_bad	Correlation length of the accel dynamic bias in [s]
tau_bgd	Correlation length of the gyro dynamic bias in [s]
e_position	Position in ECEF coordinates in [m]
e_quat_b	Quaternion from body to Earth frame
p_accelBias	Acceleration bias in platform frame coordinates [m/s^2]
p_gyroBias	Angular rate bias in platform frame coordinates [rad/s]
p_accelScale	Acceleration scale factor in platform frame coordinates [m/s^2]
p_gyroScale	Angular rate scale factor in platform frame coordinates [rad/s]
p_quatAccel_ps	Accelerometer misalignment
p_quatGyro_ps	Gyroscope misalignment
config	Integration config
accelBiases	Flag wether to calculate the rows and columns for the acceleration bias
gyroBiases	Flag wether to calculate the rows and columns for the angular rate bias
accelScaleFactor	Flag wether to calculate the rows and columns for the acceleration scale factor
gyroScaleFactor	Flag wether to calculate the rows and columns for the angular rate scale factor
accelMisalignment	Flag wether to calculate the rows and columns for the accelerometer misalignment
gyroMisalignment	Flag wether to calculate the rows and columns for the gyroscope misalignment