so3.c File Reference

Single-precision SO(3) implementation for Cortex-M4F. More...

#include "so3.h"
#include <math.h>

Include dependency graph for so3.c:

Go to the source code of this file.

Functions
void	so3_skew (const float v[3], float S[9])
void	so3_exp (const float phi[3], float R[9])
float	vec3_dot (const float a[3], const float b[3])
float	vec3_norm (const float v[3])
void	vec3_cross (const float a[3], const float b[3], float c[3])
int	vec3_normalize (float v[3])
	0 = OK
void	so3_orthonormalize (float R[9])
void	so3_to_quat (const float R[9], float q[4])
void	mat3_mul (const float A[9], const float B[9], float C[9])
	C = A*B
void	mat3_vec (const float A[9], const float x[3], float y[3])
	y = A*x
void	mat3t_vec (const float A[9], const float x[3], float y[3])
	y = A'*x
int	mat3_inv (const float A[9], float B[9])
	0 = OK

Detailed Description

Single-precision SO(3) implementation for Cortex-M4F.

Deliberate differences w.r.t. expSO3.m (double):

• small-angle threshold at 0.05 rad with a 4th-order Taylor series: in float, (1-cos t)/t^2 computed directly loses all digits for t ~ 1e-3, which is exactly the order of omega*dt at 100 Hz and above;
• so3_orthonormalize(): in single precision the product R*exp(.) accumulates orthogonality error at every step and must be re-projected onto SO(3).

Definition in file so3.c.

Function Documentation

so3_skew()

void so3_skew	(	const float	v[3],
		float	S[9]
	)

Definition at line 15 of file so3.c.

Referenced by reset_covariance(), and vector_update().

Here is the caller graph for this function:

so3_exp()

void so3_exp	(	const float	phi[3],
		float	R[9]
	)

Definition at line 22 of file so3.c.

References b.

Referenced by iekf_propagate_dtheta(), and vector_update().

Here is the caller graph for this function:

vec3_dot()

float vec3_dot	(	const float	a[3],
		const float	b[3]
	)

Definition at line 46 of file so3.c.

References b.

Referenced by calib_finish(), direction_noise_std(), iekf_init(), iekf_update_acc(), iekf_update_mag_ex(), so3_orthonormalize(), tangent_part(), vec3_norm(), and vector_update().

Here is the caller graph for this function:

vec3_norm()

float vec3_norm

(

const float

v[3]

)

Definition at line 47 of file so3.c.

References vec3_dot().

Referenced by calib_add(), calib_finish(), direction_noise_std(), iekf_update_acc(), iekf_update_mag_ex(), on_acc_window(), on_mag(), and vec3_normalize().

Here is the call graph for this function:

Here is the caller graph for this function:

vec3_cross()

void vec3_cross	(	const float	a[3],
		const float	b[3],
		float	c[3]
	)

Definition at line 49 of file so3.c.

References b.

Referenced by calib_finish(), on_mag(), and so3_orthonormalize().

Here is the caller graph for this function:

vec3_normalize()

int vec3_normalize

(

float

v[3]

)

0 = OK

Definition at line 56 of file so3.c.

References n, and vec3_norm().

Referenced by calib_finish(), direction_noise_std(), iekf_init(), project_gain(), and so3_orthonormalize().

Here is the call graph for this function:

Here is the caller graph for this function:

so3_orthonormalize()

void so3_orthonormalize

(

float

R[9]

)

Definition at line 65 of file so3.c.

References vec3_cross(), vec3_dot(), and vec3_normalize().

Referenced by iekf_init(), iekf_propagate_dtheta(), iekf_reset_attitude(), and vector_update().

Here is the call graph for this function:

Here is the caller graph for this function:

so3_to_quat()

void so3_to_quat	(	const float	R[9],
		float	q[4]
	)

Definition at line 80 of file so3.c.

References n.

Referenced by iekf_get_quat(), and tlm_build_init().

Here is the caller graph for this function:

mat3_mul()

void mat3_mul	(	const float	A[9],
		const float	B[9],
		float	C[9]
	)

C = A*B

Definition at line 106 of file so3.c.

References A, B, and C.

Referenced by accwin_rotate(), iekf_propagate_dtheta(), reset_covariance(), and vector_update().

Here is the caller graph for this function:

mat3_vec()

void mat3_vec	(	const float	A[9],
		const float	x[3],
		float	y[3]
	)

y = A*x

Definition at line 113 of file so3.c.

References A.

Referenced by accwin_add(), calib_finish(), iekf_update_acc(), iekf_update_mag_ex(), and vector_update().

Here is the caller graph for this function:

mat3t_vec()

void mat3t_vec	(	const float	A[9],
		const float	x[3],
		float	y[3]
	)

y = A'*x

Definition at line 120 of file so3.c.

References A.

Referenced by accwin_mean(), iekf_init(), iekf_update_acc(), iekf_update_mag_ex(), and project_gain().

Here is the caller graph for this function:

mat3_inv()

int mat3_inv	(	const float	A[9],
		float	B[9]
	)

0 = OK

Definition at line 127 of file so3.c.

References A, B, and id.

Referenced by vector_update().

Here is the caller graph for this function: