amino  1.0-beta2 Lightweight Robot Utility Library
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);

#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)
Function signature to check whether a differential equation has reached its goal. More...

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)
Function signature for a fixed integration step.

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)
Function signature for an adaptive integration step.

## 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 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)

static long aa_lmodulo (long a, long b)

static int64_t aa_imodulo64 (int64_t a, int64_t b)

static int aa_iremainder (int a, int b)

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 double aa_frand_minmax (double min, double max)
uniform pseudo-random in [min,max]

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.

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)

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)

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.

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_np (size_t m, size_t n, const double *A, const double *A_star, double *B)
Computer Nullspace Projection Matrix. 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)

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)

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)
Solve an ordinary differential equation.

AA_API int aa_ode_path (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, struct aa_mem_region *region, size_t *n_points, double **path)
Compute the path followed during integration.

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)
Euler / Runge-Kutta-1 integration. More...

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.

amino/tf.h
amino/la.h

Definition in file math.h.

## ◆ AA_MATCOL

 #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 339 of file math.h.

## ◆ AA_MATREF

 #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 348 of file math.h.

## ◆ AA_MAX

 #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.

## ◆ AA_MIN

 #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.

## ◆ aa_ode_check

 typedef int aa_ode_check(void *cx, double t, double *AA_RESTRICT x, double *AA_RESTRICT y)

Function signature to check whether a differential equation has reached its goal.

Return zero if not at goal. Return non-zero if goal is reached.

Definition at line 845 of file math.h.

## ◆ aa_sys_fun

 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 804 of file math.h.

## ◆ aa_ode_integrator

 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 811 of file math.h.

## ◆ aa_la_axpy()

 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_la_axpy3()

 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_la_care_laub()

 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_la_cross()

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

Cross product.

$c \leftarrow a \times b$

## ◆ aa_la_dist()

 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_la_dls()

 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_la_dlsnp()

 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_la_dot()

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

Dot product.

${\mathbf x}^T {\mathbf y}$

## ◆ aa_la_dpinv()

 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_la_dzdpinv()

 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_la_inv()

 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

## ◆ aa_la_inverse3x3()

 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 547 of file math.h.

## ◆ aa_la_inverse3x3_()

 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_la_linterp()

 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_la_lls()

 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_la_norm()

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

Euclidean norm of x.

$\sqrt{ {\mathbf x}^T {\mathbf y} }$

## ◆ aa_la_normalize()

 AA_API void aa_la_normalize ( size_t n, double * x )

Make x unit vector.

$x \leftarrow \frac{x}{\|x\|}$

## ◆ aa_la_np()

 AA_API void aa_la_np ( size_t m, size_t n, const double * A, const double * A_star, double * B )

Computer Nullspace Projection Matrix.

$B = (A^* A) - I$

Parameters
 m rows in A n cols in A A matrix A_star pseudoinverse of A B nullspace projection matrix

## ◆ aa_la_plane_fit()

 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_la_plane_hessian()

 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_la_point_plane()

 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 )

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 )

$r_i \leftarrow \alpha + x_i$

## ◆ aa_la_scal()

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

vector-scalar multiplication.

$x_i \leftarrow \alpha * x_i$

## ◆ aa_la_sdiv()

 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_la_sinc()

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

increment by scalar.

$x_i \leftarrow \alpha + x_i$

## ◆ aa_la_smul()

 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_la_ssd()

 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_la_ssub()

 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_la_svd()

 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 )

$r_i \leftarrow x_i + y_i$

## ◆ aa_la_vdiv()

 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_la_vinc()

 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_la_vmul()

 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_la_vsub()

 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_la_wdot()

 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_la_xlsnp()

 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 - (A^* A - I) 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_star $$A \in \Re^m\times\Re^n$$ b $$b \in \Re^m$$ xp $$x \in \Re^n$$ x $$x \in \Re^n$$

## ◆ aa_lsim_dstep()

 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_lsim_estep()

 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_lsim_rk4step()

 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_odestep_dorpri45()

 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_odestep_euler()

 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_odestep_rk1()

 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 )

Euler / Runge-Kutta-1 integration.

$x_1 \leftarrow dt*dx + x_0$

## ◆ aa_odestep_rk2()

 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_odestep_rk4()

 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_odestep_rkbs23()

 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_odestep_rkck45()

 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_odestep_rkf45()

 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

## ◆ aa_sincos()

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

Portable sincos.

GCC will usually optimize to sincos()

Definition at line 192 of file math.h.

## ◆ aa_stat_box_muller()

 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_stat_excluded_circ_mean_std()

 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_stat_excluded_mean_std()

 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 max_iterations maximum number of iterations

## ◆ aa_stat_vmean()

 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_stat_vmean_cov()

 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_sys_affine()

 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$