math.h File Reference

Numerical routines. More...

Go to the source code of this file.

Classes
struct	aa_ode_sol_opts
	Options for the differential equation solver. More...
struct	aa_sys_affine_t
	Context-struct for function aa_sys_affine. More...

Macros
#define	AA_EPSILON   .001
	a small number
#define	AA_MAX(a, b)
	maximum of a and b More...
#define	AA_MIN(a, b)
	minimum of a and b More...
#define	AA_MODULO(a, b)   (((a) % (b)) + (b)) % (b);
	Fortran modulo, Ada mod.
#define	AA_MATCOL(A, lda, col)   ((A)+(col)*(lda))
	Pointer to a column of a matrix. More...
#define	AA_MATREF(A, lda, row, col)   (AA_MATCOL(A,lda,col)[row])
	Reference an element in a column-major matrix. More...
#define	AA_ODE_EULER   AA_ODE_RK1
	Alias for Runge-Kutta-1 integration.
#define	AA_ODE_HEUN   AA_ODE_RK2
	Alias for Runge-Kutta-2 integration.

Typedefs
typedef void	aa_sys_fun (const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)
	A "Signal" function. More...
typedef int	aa_ode_check (void *cx, double t, double *AA_RESTRICT x, double *AA_RESTRICT y)
typedef void	aa_odestep_fixed (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
typedef void	aa_odestep_adaptive (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x_n_1, double *AA_RESTRICT x_n)

Enumerations
enum	aa_ode_integrator {   AA_ODE_RK1, AA_ODE_RK2, AA_ODE_RK4, AA_ODE_RK45_F,   AA_ODE_RK45_CK, AA_ODE_RK45_DP, AA_ODE_RK23_BS }
	The known integration methods. More...

Functions
static AA_DEPRECATED double	aa_clamp (double val, double level)
	force value to be within +/- level
static double	aa_fclamp (double val, double min, double max)
	return val within range (min,max)
static double	aa_fdeadzone (double val, double min, double max, double deadval)
	apply deadzone to val
static void	aa_vclamp (size_t n, double *v, double min, double max)
	modify each element of v to be within range (min,max)
static double	aa_sign (double val)
	return the sign of val, one of {-1,0,1}
AA_API int	aa_isfok (double x)
	returns one if x is not infinity or NAN
static int	aa_feq (double a, double b, double tol)
	Fuzzy equals.
AA_API int	aa_veq (size_t n, const double *a, const double *b, double tol)
	Fuzzy equals.
static double	aa_fsq (double a)
	square
static int	aa_imodulo (int a, int b)
	Fortran modulo, Ada mod.
static long	aa_lmodulo (long a, long b)
	Fortran modulo, Ada mod.
static int64_t	aa_imodulo64 (int64_t a, int64_t b)
	Fortran modulo, Ada mod.
static int	aa_iremainder (int a, int b)
	Fortran mod, Ada rem.
static double	aa_fmodulo (double a, double b)
	Mathematical modulo, Fortran modulo, Ada mod.
static double	aa_fremainder (double a, double b)
	Mathematical remainder, Fortran mod, Ada rem.
AA_API size_t	aa_fminloc (size_t n, double *v)
	Returns index of minimum element in array v.
AA_API size_t	aa_fmaxloc (size_t n, double *v)
	Returns index of maximum element in array v.
AA_API double	aa_frand ()
	uniform pseudo-random in [0,1.0]
AA_API void	aa_vrand (size_t n, double *v)
	fills v with random numbers in [0,1.0]
static double	aa_horner3 (double x, double a0, double a1, double a2)
	Evaluate three-term polynomial using horner's rule.
static void	aa_sincos (double x, double *s, double *c)
	Portable sincos. More...
AA_API void	aa_stat_box_muller (double x1, double x2, double *z1, double *z2)
	Generate 2 gaussian random numbers with stddev=1 from two uniform random numbers in interval (0,1]. More...
static double	aa_stat_z2x (double z, double mu, double sigma)
	Convert z-score to x-score a normal distribution.
static double	aa_stat_x2z (double x, double mu, double sigma)
	Convert x-score to z-score a normal distribution.
AA_API double	aa_stat_mean (size_t n, const double *x)
	Compute mean of vector x.
AA_API double	aa_stat_std (size_t n, const double *x)
	Compute standard deviation of vector x.
AA_API size_t	aa_stat_excluded_mean_std (size_t n, const double *x, double *pmu, double *psigma, double zmin, double zmax, size_t max_iterations)
	Compute mean and standard deviation, excluding outliers. More...
AA_API double	aa_stat_circ_mean (size_t n, const double *x)
	Compute mean of angles.
AA_API double	aa_stat_circ_std (size_t n, const double *x)
	Compute standard deviation of vector x.
AA_API size_t	aa_stat_excluded_circ_mean_std (size_t n, const double *x, double *pmu, double *psigma, double zmin, double zmax, size_t max_iterations)
	Compute mean and standard deviation, excluding outliers. More...
AA_API void	aa_stat_vmean (size_t m, size_t n, const double *X, double *mu)
	Compute mean of vectors. More...
AA_API void	aa_stat_vmean_cov (size_t m, size_t n, const double *X, double *mu, double *E)
	Compute sample covariance of vectors. More...
double	aa_stat_mahalanobis (size_t m, const double *x, const double *mu, const double *E_inv)
	Mahalanobis distance. More...
static double	aa_ang_rad2deg (double rad)
	convert radians to degrees
static double	aa_ang_deg2rad (double deg)
	convert radians to degrees
static double	aa_ang_norm_2pi (double an)
	normalize angle on interval [0,2pi)
static double	aa_ang_norm_pi (double an)
	normalize angle on interval (-pi,pi)
static double	aa_ang_delta (double a, double b)
	Difference between two angles, interval (-pi,pi)
AA_API double	aa_la_min (size_t n, const double *x)
	min of vector
AA_API double	aa_la_max (size_t n, const double *x)
	max of vector
AA_API double	aa_la_dot (size_t n, const double *x, const double *y)
	Dot product. More...
AA_API double	aa_la_norm (size_t n, const double *x)
	Euclidean norm of x. More...
AA_API double	aa_la_ssd (size_t n, const double *x, const double *y)
	Sum of Squared Differences. More...
AA_API double	aa_la_dist (size_t n, const double *x, const double *y)
	Euclidean Distance. More...
AA_API void	aa_la_sadd (size_t n, double alpha, const double *x, double *r)
	vector-scalar addition. More...
AA_API void	aa_la_scal (size_t n, double alpha, double *x)
	vector-scalar multiplication. More...
AA_API void	aa_la_vinc (size_t n, const double *x, double *y)
	increment by vector. More...
AA_API void	aa_la_sinc (size_t n, double alpha, double *x)
	increment by scalar. More...
AA_API void	aa_la_axpy (size_t n, double alpha, const double *x, double *y)
	increment by scale times vector. More...
AA_API void	aa_la_axpy3 (size_t n, double alpha, const double *x, const double *y, double *z)
	increment by scale times vector. More...
AA_API void	aa_la_smul (size_t n, double alpha, const double *x, double *r)
	vector-scalar multiplication. More...
AA_API void	aa_la_ssub (size_t n, double alpha, const double *x, double *r)
	vector-scalar subtraction. More...
AA_API void	aa_la_sdiv (size_t n, double alpha, const double *x, double *r)
	vector-scalar division. More...
AA_API void	aa_la_vadd (size_t n, const double *x, const double *y, double *r)
	Elementwise addition. More...
AA_API void	aa_la_vsub (size_t n, const double *x, const double *y, double *r)
	Elementwise subtraction. More...
AA_API void	aa_la_vmul (size_t n, const double *x, const double *y, double *r)
	Elementwise multiplication. More...
AA_API void	aa_la_vdiv (size_t n, const double *x, const double *y, double *r)
	Elementwise division. More...
AA_API void	aa_la_cross (const double a[3], const double b[3], double c[3])
	Cross product. More...
AA_API void	aa_la_normalize (size_t n, double *x)
	Make x unit vector. More...
AA_API double	aa_la_point_plane (size_t n, const double *point, const double *plane)
	Point Plane Distance. More...
AA_API void	aa_la_transpose (size_t n, double *A)
	transpose square matrix A in place
AA_API void	aa_la_transpose2 (size_t m, size_t n, const double *A, double *At)
	transpose m*n matrix A into n*m matrix At
static void	aa_la_diag (size_t n, double *A, double x)
	Set diagonal of A to x. More...
static void	aa_la_ident (size_t n, double *A)
	Set A to the identity matrix.
static void	aa_la_mvmul (size_t m, size_t n, const double *A, const double *x, double *b)
	matrix-vector multiplication
AA_API double	aa_la_wdot (size_t n, const double *x, const double *A, const double *y)
	Weighted inner product. More...
AA_API int	aa_la_svd (size_t m, size_t n, const double *A, double *U, double *S, double *Vt)
	Singular Value Decomposition of A. More...
AA_API int	aa_la_inv (size_t n, double *A)
	Inverse of A. More...
AA_API void	aa_la_inverse3x3_ (const double R[9], double S[9])
	Inverse of 3x3 matrix R. More...
static void	aa_la_inverse3x3 (const double R[9], double S[9])
	Inverse of 3x3 matrix R. More...
double	aa_la_det3x3 (const double R[AA_RESTRICT 9])
	Determinant of 3x3 matrix R.
AA_API double	aa_la_trace (size_t n, const double *A)
	Trace of a matrix.
AA_API void	aa_la_dpinv (size_t m, size_t n, double k, const double *A, double *A_star)
	Damped Pseudo Inverse of A. More...
AA_API void	aa_la_dzdpinv (size_t m, size_t n, double s2_min, const double *A, double *A_star)
	Deadzone, Damped Pseudo Inverse of A. More...
AA_API void	aa_la_dls (size_t m, size_t n, double k, const double *A, const double *b, double *x)
	Damped Least Squares. More...
AA_API void	aa_la_xlsnp (size_t m, size_t n, const double *A, const double *A_star, const double *b, const double *xp, double *x)
	Least Squares with Nullspace projection. More...
AA_API void	aa_la_dlsnp (size_t m, size_t n, double k, const double *A, const double *b, const double *xp, double *x)
	Damped Least Squares with Nullspace projection. More...
AA_API void	aa_la_lls (size_t m, size_t n, size_t p, const double *A, const double *b, double *x)
	Linear Least Squares. More...
AA_API int	aa_la_care_laub (size_t m, size_t n, size_t p, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT C, double *AA_RESTRICT X)
	Solve the continuous-time Riccati equation. More...
AA_API void	aa_la_linterp (size_t n, double t0, const double *X0, double t1, const double *X1, double ti, double *Xi)
	Linear interpolation. More...
AA_API void	aa_la_quadterp (size_t n, double t0, const double *X0, double t1, const double *X1, double t2, const double *X2, double ti, double *Xi)
	Quadratic interpolation. More...
AA_API void	aa_la_quadterp_dx (size_t n, double t0, const double *X0, double t1, const double *X1, double t2, const double *X2, double ti, double *dXi)
	Quadratic interpolation, derivative.
AA_API void	aa_la_plane_hessian (size_t n, double *plane)
	Convert plane to hessian normal form. More...
AA_API void	aa_la_plane_fit (size_t m, size_t n, const double *points, double *plane)
	Fit a plane to a set of points. More...
AA_API void	aa_lsim_dstep (size_t m, size_t n, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT x0, const double *AA_RESTRICT u, double *AA_RESTRICT x1)
	Linear simulation step in discrete time. More...
AA_API void	aa_lsim_estep (size_t m, size_t n, double dt, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT x0, const double *AA_RESTRICT u, double *AA_RESTRICT x1)
	Linear simulation step with euler integration. More...
AA_API int	aa_ode_sol (enum aa_ode_integrator integrator, const struct aa_ode_sol_opts *AA_RESTRICT opts, size_t n, aa_sys_fun sys, const void *sys_cx, aa_ode_check check, void *check_cx, double t0, double dt0, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
AA_API void	aa_odestep_euler (size_t n, double dt, const double *AA_RESTRICT dx, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
	Euler / Runge-Kutta-1 integration. More...
AA_API void	aa_odestep_rk1 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
AA_API void	aa_odestep_rk2 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
	Runge-Kutta-2 (Heun's Method) integration. More...
AA_API void	aa_odestep_rk4 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
	Runge-Kutta-4 integration. More...
AA_API void	aa_odestep_rkf45 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
	Runge-Kutta-4-5 (Fehlberg Method) integration. More...
AA_API void	aa_odestep_rkck45 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
	Runge-Kutta-4-5 (Cash-Karp Method) integration. More...
AA_API void	aa_odestep_dorpri45 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
	Runge-Kutta-4-5 (Dormand-Prince Method) integration. More...
AA_API void	aa_odestep_rkbs23 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
	Runge-Kutta-2-3 (Bogacki-Shampine Method) integration. More...
AA_API void	aa_lsim_rk4step (size_t m, size_t n, double dt, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT x0, const double *AA_RESTRICT u, double *AA_RESTRICT x1)
	Linear simulation step with Runge-Kutta-4 integration. More...
AA_API void	aa_sys_affine (const aa_sys_affine_t *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT dx)
	Affine system model function. More...

Detailed Description

Numerical routines.

See also

amino/tf.h

amino/la.h

Definition in file math.h.

Macro Definition Documentation

#define AA_MATCOL	(		A,
			lda,
			col
	)		((A)+(col)*(lda))

Pointer to a column of a matrix.

Parameters

A	Matrix pointer
lda	Leading dimension of A
col	column of the matrix (indexed from zero)

Definition at line 338 of file math.h.

#define AA_MATREF	(		A,
			lda,
			row,
			col
	)		(AA_MATCOL(A,lda,col)[row])

Reference an element in a column-major matrix.

Parameters

A	Matrix pointer
lda	Leading dimension of A
row	row of the matrix (indexed from zero)
col	column of the matrix (indexed from zero)

Definition at line 347 of file math.h.

#define AA_MAX	(		a,
			b
	)

Value:

({ const __typeof__(a) aa_$_max_a = (a); \
       const __typeof__(b) aa_$_max_b = (b); \
       (aa_$_max_a > aa_$_max_b) ? aa_$_max_a : aa_$_max_b; })

maximum of a and b

Definition at line 63 of file math.h.

#define AA_MIN	(		a,
			b
	)

Value:

({ const __typeof__(a) aa_$_min_a = (a); \
       const __typeof__(b) aa_$_min_b = (b); \
       (aa_$_min_a < aa_$_min_b) ? aa_$_min_a : aa_$_min_b; })

minimum of a and b

Definition at line 69 of file math.h.

Typedef Documentation

typedef void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)

A "Signal" function.

\[ y = f(cx, x) \]

Definition at line 788 of file math.h.

Enumeration Type Documentation

enum aa_ode_integrator

The known integration methods.

Enumerator
AA_ODE_RK1	Runge-Kutta 1 integration.
AA_ODE_RK2	Runge-Kutta 2 integration.
AA_ODE_RK4	Runge-Kutta 4 integration.
AA_ODE_RK45_F	Runge-Kutta-Fehlberg integration.
AA_ODE_RK45_CK	Runge-Kutta 4-5 / Cash Karp integration.
AA_ODE_RK45_DP	Runge-Kutta 4-5 / Dormand-Prince integration.
AA_ODE_RK23_BS	Runge-Kutta 2-3 / Bogacki-Shampine integration.

Definition at line 795 of file math.h.

Function Documentation

AA_API void aa_la_axpy	(	size_t	n,
		double	alpha,
		const double *	x,
		double *	y
	)

increment by scale times vector.

\[ y_i \leftarrow \alpha x_i + y_i \]

AA_API void aa_la_axpy3	(	size_t	n,
		double	alpha,
		const double *	x,
		const double *	y,
		double *	z
	)

increment by scale times vector.

\[ z_i \leftarrow \alpha x_i + y_i \]

three address version of the regular axpy

AA_API int aa_la_care_laub	(	size_t	m,
		size_t	n,
		size_t	p,
		const double *AA_RESTRICT	A,
		const double *AA_RESTRICT	B,
		const double *AA_RESTRICT	C,
		double *AA_RESTRICT	X
	)

Solve the continuous-time Riccati equation.

\[ A^TX + XA - XBX + C = 0 \]

See Laub, Alan. "A Schur Method for Solving Algebraic Riccati Equations". IEEE Transactions on Automatic Control. Dec 1979.

AA_API void aa_la_cross	(	const double	a[3],
		const double	b[3],
		double	c[3]
	)

Cross product.

\[ c \leftarrow a \times b \]

static void aa_la_diag ( size_t  n, double *  A, double  x  )

inlinestatic

Set diagonal of A to x.

Definition at line 482 of file math.h.

AA_API double aa_la_dist	(	size_t	n,
		const double *	x,
		const double *	y
	)

Euclidean Distance.

\[ \sqrt{\sum_{i=0}^n (x_i-y_i)^2} \]

AA_API void aa_la_dls	(	size_t	m,
		size_t	n,
		double	k,
		const double *	A,
		const double *	b,
		double *	x
	)

Damped Least Squares.

\[ x = A^\ddagger b \]

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters

m	rows in A
n	cols in A
k	square of damping factor
A	\( A \in \Re^m\times\Re^n \)
b	\( b \in \Re^m \)
x	\( x \in \Re^n \)

AA_API void aa_la_dlsnp	(	size_t	m,
		size_t	n,
		double	k,
		const double *	A,
		const double *	b,
		const double *	xp,
		double *	x
	)

Damped Least Squares with Nullspace projection.

\[ x = A^\ddagger b + (I-A^\ddagger A)x_p \]

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters

m	rows in A
n	cols in A
k	square of damping factor
A	\( A \in \Re^m\times\Re^n \)
b	\( b \in \Re^m \)
xp	\( x \in \Re^n \)
x	\( x \in \Re^n \)

AA_API double aa_la_dot	(	size_t	n,
		const double *	x,
		const double *	y
	)

Dot product.

\[ {\mathbf x}^T {\mathbf y} \]

AA_API void aa_la_dpinv	(	size_t	m,
		size_t	n,
		double	k,
		const double *	A,
		double *	A_star
	)

Damped Pseudo Inverse of A.

\[ A^\ddagger \leftarrow A^T (AA^T + kI)^{-1} \]

\[ A^\ddagger \leftarrow \sum_{i=0}^r \frac{\sigma_i}{{\sigma_i}^2+k} v_i {u_i}^T \]

The above two formulas are equivalent. This function uses the SVD method shown in the second formula.

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters

m	rows of A
n	cols of A
A	\( A \in \Re^m\times\Re^n\)
A_star	\( A^\ddagger \in \Re^n\times\Re^m\)
k	square of damping factor

AA_API void aa_la_dzdpinv	(	size_t	m,
		size_t	n,
		double	s2_min,
		const double *	A,
		double *	A_star
	)

Deadzone, Damped Pseudo Inverse of A.

\[ A^\ddagger \leftarrow \sum_{i=0}^r \frac{\sigma_i}{{\sigma_i}^2+k} v_i {u_i}^T \]

The denominator \({\sigma_i}^2 \) goes to zero near singularities. This function fixes the minimum value of the denominator at parameter s2_min.

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters

m	rows of A
n	cols of A
A	\( A \in \Re^m\times\Re^n\)
A_star	\( A^\ddagger \in \Re^n\times\Re^m\)
s2_min	Minimum acceptable value for squared Singular Value

AA_API int aa_la_inv	(	size_t	n,
		double *	A
	)

Inverse of A.

\[ A \leftarrow A^{-1} \]

Parameters

A	\(A \in \Re^n \times \Re^n\), column major
n	rows and columns

static void aa_la_inverse3x3 ( const double  R[9], double  S[9]  )

inlinestatic

Inverse of 3x3 matrix R.

wrapper for the maxima-generated function

Definition at line 546 of file math.h.

AA_API void aa_la_inverse3x3_	(	const double	R[9],
		double	S[9]
	)

Inverse of 3x3 matrix R.

This function is generated by Maxima.

AA_API void aa_la_linterp	(	size_t	n,
		double	t0,
		const double *	X0,
		double	t1,
		const double *	X1,
		double	ti,
		double *	Xi
	)

Linear interpolation.

Parameters

n	size of the X vectors
t0	independent variable
X0	dependent variable at t0
t1	independent variable
X1	dependent variable at t1
ti	independent variable, interpolation point
Xi	dependent variable, interpolated values

AA_API void aa_la_lls	(	size_t	m,
		size_t	n,
		size_t	p,
		const double *	A,
		const double *	b,
		double *	x
	)

Linear Least Squares.

\[ b = Ax \]

Solves for x.

Parameters

m	rows in A
n	cols in A
p	cols in b and x
A	matrix
b	offset matrix
x	solution matrix

AA_API double aa_la_norm	(	size_t	n,
		const double *	x
	)

Euclidean norm of x.

\[ \sqrt{ {\mathbf x}^T {\mathbf y} } \]

AA_API void aa_la_normalize	(	size_t	n,
		double *	x
	)

Make x unit vector.

\[ x \leftarrow \frac{x}{\|x\|}\]

AA_API void aa_la_plane_fit	(	size_t	m,
		size_t	n,
		const double *	points,
		double *	plane
	)

Fit a plane to a set of points.

Parameters

m	size of space
n	number of points
points	m*n matrix of points, point per column, column major
plane	vector length m+1 to hold plane coefficients in hessian-normal form

AA_API void aa_la_plane_hessian	(	size_t	n,
		double *	plane
	)

Convert plane to hessian normal form.

Parameters

plane	Coefficients for plane
n	length of plane vector, size of plane space is n-1

AA_API double aa_la_point_plane	(	size_t	n,
		const double *	point,
		const double *	plane
	)

Point Plane Distance.

Parameters

n	size of space
point	vector of length n
plane	vector of length n+1

AA_API void aa_la_quadterp	(	size_t	n,
		double	t0,
		const double *	X0,
		double	t1,
		const double *	X1,
		double	t2,
		const double *	X2,
		double	ti,
		double *	Xi
	)

Quadratic interpolation.

Parameters

n	size of the X vectors
t0	independent variable
X0	dependent variable at t0
t1	independent variable
X1	dependent variable at t1
t2	independent variable
X2	dependent variable at t2
ti	independent variable, interpolation point
Xi	dependent variable, interpolated values

AA_API void aa_la_sadd	(	size_t	n,
		double	alpha,
		const double *	x,
		double *	r
	)

vector-scalar addition.

\[ r_i \leftarrow \alpha + x_i \]

AA_API void aa_la_scal	(	size_t	n,
		double	alpha,
		double *	x
	)

vector-scalar multiplication.

\[ x_i \leftarrow \alpha * x_i \]

AA_API void aa_la_sdiv	(	size_t	n,
		double	alpha,
		const double *	x,
		double *	r
	)

vector-scalar division.

\[ r_i \leftarrow \alpha / x_i \]

AA_API void aa_la_sinc	(	size_t	n,
		double	alpha,
		double *	x
	)

increment by scalar.

\[ x_i \leftarrow \alpha + x_i \]

AA_API void aa_la_smul	(	size_t	n,
		double	alpha,
		const double *	x,
		double *	r
	)

vector-scalar multiplication.

\[ r_i \leftarrow \alpha * x_i \]

AA_API double aa_la_ssd	(	size_t	n,
		const double *	x,
		const double *	y
	)

Sum of Squared Differences.

\[ \sum_{i=0}^n (x_i-y_i)^2 \]

AA_API void aa_la_ssub	(	size_t	n,
		double	alpha,
		const double *	x,
		double *	r
	)

vector-scalar subtraction.

\[ r_i \leftarrow \alpha - x_i \]

AA_API int aa_la_svd	(	size_t	m,
		size_t	n,
		const double *	A,
		double *	U,
		double *	S,
		double *	Vt
	)

Singular Value Decomposition of A.

\[ A = U \Sigma V^T \]

Parameters

m	rows
n	columns
A	\(A \in \Re^m \times \Re^n\), column major
U	\(U \in \Re^m \times \Re^m\), column major. If null, U is returned in A. U and Vt cannot both be null.
S	\(S \in \Re^m min(m,n)\), singular values
Vt	\(V^T \in \Re^n \times \Re^n\), singular vectors. If null, Vt is returned in A. Vt and U cannot both be null.

AA_API void aa_la_vadd	(	size_t	n,
		const double *	x,
		const double *	y,
		double *	r
	)

Elementwise addition.

\[ r_i \leftarrow x_i + y_i \]

AA_API void aa_la_vdiv	(	size_t	n,
		const double *	x,
		const double *	y,
		double *	r
	)

Elementwise division.

\[ r_i \leftarrow x_i / y_i \]

AA_API void aa_la_vinc	(	size_t	n,
		const double *	x,
		double *	y
	)

increment by vector.

\[ y_i \leftarrow x_i + y_i \]

AA_API void aa_la_vmul	(	size_t	n,
		const double *	x,
		const double *	y,
		double *	r
	)

Elementwise multiplication.

\[ r_i \leftarrow x_i * y_i \]

AA_API void aa_la_vsub	(	size_t	n,
		const double *	x,
		const double *	y,
		double *	r
	)

Elementwise subtraction.

\[ r_i \leftarrow x_i - y_i \]

AA_API double aa_la_wdot	(	size_t	n,
		const double *	x,
		const double *	A,
		const double *	y
	)

Weighted inner product.

\[ x^T A y \]

AA_API void aa_la_xlsnp	(	size_t	m,
		size_t	n,
		const double *	A,
		const double *	A_star,
		const double *	b,
		const double *	xp,
		double *	x
	)

Least Squares with Nullspace projection.

\[ x = A^* b + (I-A^* A) x_p \]

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters

m	rows in A
n	cols in A
A	\( A \in \Re^m\times\Re^n \)
A*	\( A \in \Re^m\times\Re^n \)
b	\( b \in \Re^m \)
xp	\( x \in \Re^n \)
x	\( x \in \Re^n \)

AA_API void aa_lsim_dstep	(	size_t	m,
		size_t	n,
		const double *AA_RESTRICT	A,
		const double *AA_RESTRICT	B,
		const double *AA_RESTRICT	x0,
		const double *AA_RESTRICT	u,
		double *AA_RESTRICT	x1
	)

Linear simulation step in discrete time.

\[ x_1 \leftarrow Ax_0 + Bu \]

Parameters

m	state size
n	input size
A	process model, m*m matrix
B	input model, m*n matrix
u	input, n length vector
x0	initial state, m length vector
x1	resulting state, m length vector

AA_API void aa_lsim_estep	(	size_t	m,
		size_t	n,
		double	dt,
		const double *AA_RESTRICT	A,
		const double *AA_RESTRICT	B,
		const double *AA_RESTRICT	x0,
		const double *AA_RESTRICT	u,
		double *AA_RESTRICT	x1
	)

Linear simulation step with euler integration.

\[ \dot{x} = Ax_0 + Bu \]

\[ x_1 \leftarrow x_0 + \Delta t \dot{x} \]

Parameters

m	state size
n	input size
dt	time step
A	process model, m*m matrix
B	input model, m*n matrix
u	input, n length vector
x0	initial state, m length vector
x1	resulting state, m length vector

AA_API void aa_lsim_rk4step	(	size_t	m,
		size_t	n,
		double	dt,
		const double *AA_RESTRICT	A,
		const double *AA_RESTRICT	B,
		const double *AA_RESTRICT	x0,
		const double *AA_RESTRICT	u,
		double *AA_RESTRICT	x1
	)

Linear simulation step with Runge-Kutta-4 integration.

\[ \dot{x} = Ax_0 + Bu \]

Parameters

m	state size
n	input size
dt	time step
A	process model, m*m matrix
B	input model, m*n matrix
u	input, n length vector
x0	initial state, m length vector
x1	resulting state, m length vector

AA_API void aa_odestep_dorpri45	(	size_t	n,
		aa_sys_fun	sys,
		const void *	cx,
		double	t0,
		double	dt,
		const double *AA_RESTRICT	x0,
		double *AA_RESTRICT	k,
		double *AA_RESTRICT	x4,
		double *AA_RESTRICT	x5
	)

Runge-Kutta-4-5 (Dormand-Prince Method) integration.

See Dormand, J. R.; Prince, P. J. (1980), "A family of embedded Runge-Kutta formulae", Journal of Computational and Applied Mathematics 6 (1): 19–26,

Parameters

n	state size
sys	function to integrate
cx	Context struct for sys
t0	time
dt	time step
x0	initial state
k	derivitive matix,size n*6, initially first column contains sys evaluated at x0, overwritten with sys evalutated at x4
x4	fourth-order step
x5	fifth-order step

Precondition

• First (initial) column of n*6 matrix k contains sys evalutated at x0

Postcondition

• First (initial) column of n*6 matrix k unmodified
• Sixth (last) column of n*6 matrix k contains sys evalutated at x4

AA_API void aa_odestep_euler	(	size_t	n,
		double	dt,
		const double *AA_RESTRICT	dx,
		const double *AA_RESTRICT	x0,
		double *AA_RESTRICT	x1
	)

Euler / Runge-Kutta-1 integration.

\[ x_1 \leftarrow dt*dx + x_0 \]

AA_API void aa_odestep_rk2	(	size_t	n,
		aa_sys_fun	sys,
		const void *	cx,
		double	t0,
		double	dt,
		const double *AA_RESTRICT	x0,
		double *AA_RESTRICT	x1
	)

Runge-Kutta-2 (Heun's Method) integration.

Parameters

n	state size
sys	function to integrate
cx	Context struct for sys
t0	time
dt	time step
x0	initial state
x1	integrated state

AA_API void aa_odestep_rk4	(	size_t	n,
		aa_sys_fun	sys,
		const void *	cx,
		double	t0,
		double	dt,
		const double *AA_RESTRICT	x0,
		double *AA_RESTRICT	x1
	)

Runge-Kutta-4 integration.

Parameters

n	state size
sys	function to integrate
cx	Context struct for sys
t0	time
dt	time step
x0	initial state
x1	integrated state

AA_API void aa_odestep_rkbs23	(	size_t	n,
		aa_sys_fun	sys,
		const void *	cx,
		double	t0,
		double	dt,
		const double *AA_RESTRICT	x0,
		double *AA_RESTRICT	k,
		double *AA_RESTRICT	x4,
		double *AA_RESTRICT	x5
	)

Runge-Kutta-2-3 (Bogacki-Shampine Method) integration.

See Bogacki, Przemyslaw; Shampine, Lawrence F. (1989), "A 3(2) pair of Runge–Kutta formulas", Applied Mathematics Letters 2 (4)

Parameters

n	state size
sys	function to integrate
cx	Context struct for sys
t0	time
dt	time step
x0	initial state
k	derivitive matix,size n*4, initially first column contains sys evaluated at x0, overwritten with sys evalutated at x4
x4	fourth-order step
x5	fifth-order step

Precondition

• First (initial) column of n*4 matrix k contains sys evalutated at x0

Postcondition

• First (initial) column of n*4 matrix k unmodified
• Fourth (last) column of n*4 matrix k contains sys evalutated at x4

AA_API void aa_odestep_rkck45	(	size_t	n,
		aa_sys_fun	sys,
		const void *	cx,
		double	t0,
		double	dt,
		const double *AA_RESTRICT	x0,
		double *AA_RESTRICT	k,
		double *AA_RESTRICT	x4,
		double *AA_RESTRICT	x5
	)

Runge-Kutta-4-5 (Cash-Karp Method) integration.

See J. R. Cash, A. H. Karp. "A variable order Runge-Kutta method for initial value problems with rapidly varying right-hand sides", ACM Transactions on Mathematical Software 16: 201-222, 1990.

Parameters

n	state size
sys	function to integrate
cx	Context struct for sys
t0	time
dt	time step
x0	initial state
k	derivitive matix,size n*5, initially first column contains sys evaluated at x0
x4	fourth-order step
x5	fifth-order step

Precondition

• First (initial) column of n*5 matrix k contains sys evalutated at x0

Postcondition

• First (initial) column of n*5 matrix k unmodified

AA_API void aa_odestep_rkf45	(	size_t	n,
		aa_sys_fun	sys,
		const void *	cx,
		double	t0,
		double	dt,
		const double *AA_RESTRICT	x0,
		double *AA_RESTRICT	k,
		double *AA_RESTRICT	x4,
		double *AA_RESTRICT	x5
	)

Runge-Kutta-4-5 (Fehlberg Method) integration.

See Erwin Fehlberg (1969). Low-order classical Runge-Kutta formulas with step size control and their application to some heat transfer problems. NASA Technical Report 315.

Parameters

n	state size
sys	function to integrate
cx	Context struct for sys
t0	time
dt	time step
x0	initial state
k	derivitive matix,size n*5, initially first column contains sys evaluated at x0
x4	fourth-order step
x5	fifth-order step

Precondition

• First (initial) column of n*5 matrix k contains sys evalutated at x0

Postcondition

• First (initial) column of n*5 matrix k unmodified

static void aa_sincos ( double  x, double *  s, double *  c  )

inlinestatic

Portable sincos.

GCC will usually optimize to sincos()

Definition at line 189 of file math.h.

AA_API void aa_stat_box_muller	(	double	x1,
		double	x2,
		double *	z1,
		double *	z2
	)

Generate 2 gaussian random numbers with stddev=1 from two uniform random numbers in interval (0,1].

See Box, G. E. P. and Muller, M. E. "A Note on the Generation of Random Normal Deviates." Ann. Math. Stat. 29, 610-611, 1958.

AA_API size_t aa_stat_excluded_circ_mean_std	(	size_t	n,
		const double *	x,
		double *	pmu,
		double *	psigma,
		double	zmin,
		double	zmax,
		size_t	max_iterations
	)

Compute mean and standard deviation, excluding outliers.

Parameters

	n	length of x
	x	vector of angles in radians
[out]	pmu	mean
[out]	psigma	standard deviation
	zmin	exclude all outliers below zmin standard deviations
	zmax	exclude all outliers above zmax standard deviations
	max_iterations	maximum number of iterations

AA_API size_t aa_stat_excluded_mean_std	(	size_t	n,
		const double *	x,
		double *	pmu,
		double *	psigma,
		double	zmin,
		double	zmax,
		size_t	max_iterations
	)

Compute mean and standard deviation, excluding outliers.

Parameters

	n	length of x
	x	vector of data
[out]	pmu	mean
[out]	psigma	standard deviation
	zmin	exclude all outliers below zmin standard deviations
	zmax	exclude all outliers above zmax standard deviations
	zmin	exclude all outliers below zmin standard deviations
	zmax	exclude all outliers above zmax standard deviations
	max_iterations	maximum number of iterations

double aa_stat_mahalanobis	(	size_t	m,
		const double *	x,
		const double *	mu,
		const double *	E_inv
	)

Mahalanobis distance.

AA_API void aa_stat_vmean	(	size_t	m,
		size_t	n,
		const double *	X,
		double *	mu
	)

Compute mean of vectors.

Parameters

	m	size of space
	n	number of samples
	X	matrix of samples, one per column
[out]	mu	vector, length m

AA_API void aa_stat_vmean_cov	(	size_t	m,
		size_t	n,
		const double *	X,
		double *	mu,
		double *	E
	)

Compute sample covariance of vectors.

Parameters

m	size of space
n	number of samples
X	matrix of samples, one per column
mu	vector, length m
E	matrix, m*m

AA_API void aa_sys_affine	(	const aa_sys_affine_t *	cx,
		double	t,
		const double *AA_RESTRICT	x,
		double *AA_RESTRICT	dx
	)

Affine system model function.

\[ \dot{x} = Ax + D \]