/** QR decomposition
*
* using Householder method
*
* http://rosettacode.org/wiki/QR_decomposition#C
*
* @param Q square orthogonal matrix Q [m x m]
* @param R upper triangular matrix R [m x n]
* @param in pointer to the input array [m x n]
* @param m number of rows of the input matrix
* @param n number of column of the input matrix
*/
void pprz_qr_float(float **Q, float **R, float **in, int m, int n)
{
int i, k;
float _q[m][m][m];
float *q[m][m];
float _z[m][n], _z1[m][n], _z2[m][m];
MAKE_MATRIX_PTR(z, _z, m);
MAKE_MATRIX_PTR(z1, _z1, m);
MAKE_MATRIX_PTR(z2, _z2, m);
for (i = 0; i < m; i++) for (k = 0; k < m; k++) { q[i][k] = &_q[i][k][0]; }
float_mat_copy(z, in, m, n);
for (k = 0; k < n && k < m - 1; k++) {
float e[m], x[m], a, b;
float_mat_minor(z1, z, m, n, k);
float_mat_col(x, z1, m, k);
a = float_vect_norm(x, m);
if (in[k][k] > 0) { a = -a; }
for (i = 0; i < m; i++) {
e[i] = (i == k) ? 1 : 0;
e[i] = x[i] + a * e[i];
}
b = float_vect_norm(e, m);
float_vect_sdiv(e, e, b, m);
float_mat_vmul(q[k], e, m);
float_mat_mul(z, q[k], z1, m, m, n);
}
float_mat_copy(Q, q[0], m, m);
for (i = 1; i < n && i < m - 1; i++) {
float_mat_mul(z2, q[i], Q, m, m, m);
float_mat_copy(Q, z2, m, m);
}
float_mat_mul(R, Q, in, m, m, n);
float_mat_transpose_square(Q, m);
}
// Weights are based on: Coppola et al, "On-board Communication-based Relative Localization for Collision Avoidance in Micro Air Vehicle teams", 2017
void discrete_ekf_new(struct discrete_ekf *filter)
{
// P Matrix
MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
float_mat_diagonal_scal(_P, 1.f, EKF_N);
filter->P[2][2] = 0.1;
filter->P[3][3] = 0.1;
filter->P[4][4] = 0.1;
filter->P[5][5] = 0.1;
filter->P[6][6] = 0.1;
// Q Matrix
MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
float_mat_diagonal_scal(_Q, pow(0.3, 2.f), EKF_N);
filter->Q[0][0] = 0.01;
filter->Q[1][1] = 0.01;
MAKE_MATRIX_PTR(_R, filter->R, EKF_M);
float_mat_diagonal_scal(_R, pow(0.1, 2.f), EKF_M);
filter->R[0][0] = 0.2;
// Initial assumptions
float_vect_zero(filter->X, EKF_N);
filter->X[0] = 2.5; // filter->X[0] and/or filter->[1] cannot be = 0
filter->dt = 0.1;
}
/** Polynomial regression
*
* Polynomial regression is a form of linear regression in which the relationship between
* the independent variable x and the dependent variable y is modelled as an nth order polynomial.
*
* Considering the regression model:
* @f[
* y_i = c_0 + c_1 x_i + ... + c_p x_i^p + \epsilon_i (i = 1 ... n)
* @f]
* in matrix form
* @f[
* y = X.c + \epsilon
* @f]
* where
* @f[
* X_{ij} = x_i^j (i = 1 ... n; j = 1 ... p)
* @f]
* The vector of estimated polynomial regression coefficients using ordinary least squares estimation is
* @f[
* c = (X' X)^{-1} X' y
* @f]
*
* http://en.wikipedia.org/wiki/Polynomial_regression
* http://fr.wikipedia.org/wiki/R%C3%A9gression_polynomiale
* http://www.arachnoid.com/sage/polynomial.html
*
* @param[in] x pointer to the input array of independent variable X [n]
* @param[in] y pointer to the input array of dependent variable Y [n]
* @param[in] n number of input measurments
* @param[in] p degree of the output polynomial
* @param[out] c pointer to the output array of polynomial coefficients [p+1]
*/
void pprz_polyfit_float(float* x, float* y, int n, int p, float* c)
{
int i,j,k;
// Instead of solving directly (X'X)^-1 X' y
// let's build the matrices (X'X) and (X'y)
// Then element ij in (X'X) matrix is sum_{k=0,n-1} x_k^(i+j)
// and element i in (X'y) vector is sum_{k=0,n-1} x_k^i * y_k
// Finally we can solve the linear system (X'X).c = (X'y) using SVD decomposition
// First build a table of element S_i = sum_{k=0,n-1} x_k^i of dimension 2*p+1
float S[2*p + 1];
float_vect_zero(S, 2*p + 1);
// and a table of element T_i = sum_{k=0,n-1} x_k^i*y_k of dimension p+1
// make it a matrix for later use
float _T[p + 1][1];
MAKE_MATRIX_PTR(T, _T, p + 1);
float_mat_zero(T, p + 1, 1);
S[0] = n; // S_0 is always the number of input measurements
for (k = 0; k < n; k++) {
float x_tmp = x[k];
T[0][0] += y[k];
for (i = 1; i < 2*p + 1; i++) {
S[i] += x_tmp; // add element to S_i
if (i < p + 1)
T[i][0] += x_tmp*y[k]; // add element to T_i if i < p+1
x_tmp *= x[k]; // multiply x_tmp by current value of x
}
}
// Then build a [p+1 x p+1] matrix corresponding to (X'X) based on the S_i
// element ij of (X'X) is S_(i+j)
float _XtX[p + 1][p + 1];
MAKE_MATRIX_PTR(XtX, _XtX, p + 1);
for (i = 0; i < p + 1; i++) {
for (j = 0; j < p + 1; j++) {
XtX[i][j] = S[i+j];
}
}
// Solve linear system XtX.c = T after performing a SVD decomposition of XtX
// which is probably a bit overkill but looks really cool
float w[p + 1], _v[p + 1][p + 1];
MAKE_MATRIX_PTR(v, _v, p + 1);
pprz_svd_float(XtX, w, v, p + 1, p + 1);
float _c[p + 1][1];
MAKE_MATRIX_PTR(c_tmp, _c, p + 1);
pprz_svd_solve_float(c_tmp, XtX, w, v, T, p + 1, p + 1, 1);
// set output vector
for (i = 0; i < p + 1; i++)
c[i] = c_tmp[i][0];
}
/* Perform the update step
Get Kalman Gain
P12 = P * H';
K = P12/(H * P12 + R);
Update x
x = x_p + K * (z - z_p);
Update P
P = (eye(numel(x)) - K * H) * P;
*/
void discrete_ekf_update(struct discrete_ekf *filter, float *Z)
{
MAKE_MATRIX_PTR(_tmp1, filter->tmp1, EKF_N);
MAKE_MATRIX_PTR(_tmp2, filter->tmp2, EKF_N);
MAKE_MATRIX_PTR(_tmp3, filter->tmp3, EKF_N);
MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
MAKE_MATRIX_PTR(_H, filter->H, EKF_M);
MAKE_MATRIX_PTR(_Ht, filter->Ht, EKF_N);
MAKE_MATRIX_PTR(_R, filter->R, EKF_M);
// E = H * P * H' + R
float_mat_transpose(_Ht, _H, EKF_M, EKF_N); // Ht = H'
float_mat_mul(_tmp2, _P, _Ht, EKF_N, EKF_N, EKF_M); // tmp2 = P*Ht = P*H'
float_mat_mul(_tmp1, _H, _tmp2, EKF_M, EKF_N, EKF_M); // tmp1 = H*P*H'
float_mat_sum(_tmp3, _tmp1, _R, EKF_M, EKF_M); // E = tmp1(=H*P*H') + R
// K = P * H' * inv(E)
float_mat_invert(_tmp1, _tmp3, EKF_M); // tmp1 = inv(E)
float_mat_mul(_tmp3, _tmp2, _tmp1, EKF_N, EKF_M, EKF_M); // K(tmp3) = tmp2*tmp1
// P = P - K * H * P
float_mat_mul(_tmp1, _tmp3, _H, EKF_N, EKF_M, EKF_N); // tmp1 = K*H
float_mat_mul(_tmp2, _tmp1, _P, EKF_N, EKF_N, EKF_N); // tmp3 = K*H*P
float_mat_diff(_P, _P, _tmp2, EKF_N, EKF_N); // P - K*H*P
// X = X + K * err
float err[EKF_M];
float dx_err[EKF_N];
float_vect_diff(err, Z, filter->Zp, EKF_M); // err = Z - Zp
float_mat_vect_mul(dx_err, _tmp3, err, EKF_N, EKF_M); // dx_err = K*err
float_vect_sum(filter->X, filter->Xp, dx_err, EKF_N); // X = Xp + dx_err
}
/* Perform the prediction step
Predict state
x_p = f(x);
A = Jacobian of f(x)
Predict P
P = A * P * A' + Q;
Predict measure
z_p = h(x_p)
H = Jacobian of h(x)
*/
void discrete_ekf_predict(struct discrete_ekf *filter)
{
float dX[EKF_N];
MAKE_MATRIX_PTR(_tmp1, filter->tmp1, EKF_N);
MAKE_MATRIX_PTR(_tmp2, filter->tmp2, EKF_N);
MAKE_MATRIX_PTR(_tmp3, filter->tmp3, EKF_N);
MAKE_MATRIX_PTR(_H, filter->H, EKF_N);
MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
// Fetch dX and A given X and dt and input u
linear_filter(filter->X, filter->dt, dX, _tmp1); // (note: _tmp1 = A)
// Get state prediction Xp = X + dX
float_vect_sum(filter->Xp, filter->X, dX, EKF_N);
// Get measurement prediction (Zp) and Jacobian (H)
linear_measure(filter->Xp, filter->Zp, _H);
// P = A * P * A' + Q
float_mat_mul(_tmp2, _tmp1, _P, EKF_N, EKF_N, EKF_N); // tmp2(=A*P) = A(=_tmp1)*_P
float_mat_transpose_square(_tmp1, EKF_N); // tmp1 = A'
float_mat_mul(_tmp3, _tmp2, _tmp1, EKF_N, EKF_N, EKF_N); // tmp3 = tmp2*tmp1 = A*P * A'
float_mat_sum(_P, _tmp3, _Q, EKF_N, EKF_N); // P = tmp3(=A*P*A') + Q
}
/** Cholesky decomposition
*
* http://rosettacode.org/wiki/Cholesky_decomposition#C
*
* @param out pointer to the output array [n x n]
* @param in pointer to the input array [n x n]
* @param n dimension of the matrix
*/
void pprz_cholesky_float(float **out, float **in, int n)
{
int i, j, k;
float _o[n][n];
MAKE_MATRIX_PTR(o, _o, n);
float_mat_zero(o, n, n);
for (i = 0; i < n; i++) {
for (j = 0; j < (i + 1); j++) {
float s = 0;
for (k = 0; k < j; k++) {
s += o[i][k] * o[j][k];
}
o[i][j] = (i == j) ?
sqrtf(in[i][i] - s) :
(1.0 / o[j][j] * (in[i][j] - s));
}
}
float_mat_copy(out, o, n, n);
}
/**
* Fit a linear model from samples to target values.
* Effectively a wrapper for the pprz_svd_float and pprz_svd_solve_float functions.
*
* @param[in] targets The target values
* @param[in] samples The samples / feature vectors
* @param[in] D The dimensionality of the samples
* @param[in] count The number of samples
* @param[in] use_bias Whether to use the bias. Please note that params should always be of size D+1, but in case of no bias, the bias value is set to 0.
* @param[out] parameters* Parameters of the linear fit
* @param[out] fit_error* Total error of the fit
*/
void fit_linear_model(float *targets, int D, float (*samples)[D], uint16_t count, bool use_bias, float *params,
float *fit_error)
{
// We will solve systems of the form A x = b,
// where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias)
// and b = [nx1] vector with the target values.
// x in the system are the parameters for the linear regression function.
// local vars for iterating, random numbers:
int sam, d;
uint16_t n_samples = count;
uint8_t D_1 = D + 1;
// ensure that n_samples is high enough to ensure a result for a single fit:
n_samples = (n_samples < D_1) ? D_1 : n_samples;
// n_samples should not be higher than count:
n_samples = (n_samples < count) ? n_samples : count;
// initialize matrices and vectors for the full point set problem:
// this is used for determining inliers
float _AA[count][D_1];
MAKE_MATRIX_PTR(AA, _AA, count);
float _targets_all[count][1];
MAKE_MATRIX_PTR(targets_all, _targets_all, count);
for (sam = 0; sam < count; sam++) {
for (d = 0; d < D; d++) {
AA[sam][d] = samples[sam][d];
}
if (use_bias) {
AA[sam][D] = 1.0f;
} else {
AA[sam][D] = 0.0f;
}
targets_all[sam][0] = targets[sam];
}
// decompose A in u, w, v with singular value decomposition A = u * w * vT.
// u replaces A as output:
float _parameters[D_1][1];
MAKE_MATRIX_PTR(parameters, _parameters, D_1);
float w[n_samples], _v[D_1][D_1];
MAKE_MATRIX_PTR(v, _v, D_1);
// solve the system:
pprz_svd_float(AA, w, v, count, D_1);
pprz_svd_solve_float(parameters, AA, w, v, targets_all, count, D_1, 1);
// used to determine the error of a set of parameters on the whole set:
float _bb[count][1];
MAKE_MATRIX_PTR(bb, _bb, count);
float _C[count][1];
MAKE_MATRIX_PTR(C, _C, count);
// error is determined on the entire set
// bb = AA * parameters:
MAT_MUL(count, D_1, 1, bb, AA, parameters);
// subtract bu_all: C = 0 in case of perfect fit:
MAT_SUB(count, 1, C, bb, targets_all);
*fit_error = 0;
for (sam = 0; sam < count; sam++) {
*fit_error += fabsf(C[sam][0]);
}
*fit_error /= count;
for (d = 0; d < D_1; d++) {
params[d] = parameters[d][0];
}
}
/**
* Analyze a linear flow field, retrieving information such as divergence, surface roughness, focus of expansion, etc.
* @param[in] vectors The optical flow vectors
* @param[in] count The number of optical flow vectors
* @param[in] error_threshold Error used to determine inliers / outliers.
* @param[in] n_iterations Number of RANSAC iterations.
* @param[in] n_samples Number of samples used for a single fit (min. 3).
* @param[out] parameters_u* Parameters of the horizontal flow field
* @param[out] parameters_v* Parameters of the vertical flow field
* @param[out] fit_error* Total error of the finally selected fit
* @param[out] min_error_u* Error fit horizontal flow field
* @param[out] min_error_v* Error fit vertical flow field
* @param[out] n_inliers_u* Number of inliers in the horizontal flow fit.
* @param[out] n_inliers_v* Number of inliers in the vertical flow fit.
*/
void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, int *n_inliers_v)
{
// We will solve systems of the form A x = b,
// where A = [nx3] matrix with entries [x, y, 1] for each optic flow location
// and b = [nx1] vector with either the horizontal (bu) or vertical (bv) flow.
// x in the system are the parameters for the horizontal (pu) or vertical (pv) flow field.
// local vars for iterating, random numbers:
int sam, p, i_rand, si, add_si;
// ensure that n_samples is high enough to ensure a result for a single fit:
n_samples = (n_samples < MIN_SAMPLES_FIT) ? MIN_SAMPLES_FIT : n_samples;
// n_samples should not be higher than count:
n_samples = (n_samples < count) ? n_samples : count;
// initialize matrices and vectors for the full point set problem:
// this is used for determining inliers
float _AA[count][3];
MAKE_MATRIX_PTR(AA, _AA, count);
float _bu_all[count][1];
MAKE_MATRIX_PTR(bu_all, _bu_all, count);
float _bv_all[count][1];
MAKE_MATRIX_PTR(bv_all, _bv_all, count);
for (sam = 0; sam < count; sam++) {
AA[sam][0] = (float) vectors[sam].pos.x;
AA[sam][1] = (float) vectors[sam].pos.y;
AA[sam][2] = 1.0f;
bu_all[sam][0] = (float) vectors[sam].flow_x;
bv_all[sam][0] = (float) vectors[sam].flow_y;
}
// later used to determine the error of a set of parameters on the whole set:
float _bb[count][1];
MAKE_MATRIX_PTR(bb, _bb, count);
float _C[count][1];
MAKE_MATRIX_PTR(C, _C, count);
// ***************
// perform RANSAC:
// ***************
// set up variables for small linear system solved repeatedly inside RANSAC:
float _A[n_samples][3];
MAKE_MATRIX_PTR(A, _A, n_samples);
float _bu[n_samples][1];
MAKE_MATRIX_PTR(bu, _bu, n_samples);
float _bv[n_samples][1];
MAKE_MATRIX_PTR(bv, _bv, n_samples);
float w[n_samples], _v[3][3];
MAKE_MATRIX_PTR(v, _v, 3);
float _pu[3][1];
MAKE_MATRIX_PTR(pu, _pu, 3);
float _pv[3][1];
MAKE_MATRIX_PTR(pv, _pv, 3);
// iterate and store parameters, errors, inliers per fit:
float PU[n_iterations * 3];
float PV[n_iterations * 3];
float errors_pu[n_iterations];
errors_pu[0] = 0.0;
float errors_pv[n_iterations];
errors_pv[0] = 0.0;
int n_inliers_pu[n_iterations];
int n_inliers_pv[n_iterations];
int it, ii;
for (it = 0; it < n_iterations; it++) {
// select a random sample of n_sample points:
int sample_indices[n_samples];
i_rand = 0;
// sampling without replacement:
while (i_rand < n_samples) {
si = rand() % count;
add_si = 1;
for (ii = 0; ii < i_rand; ii++) {
if (sample_indices[ii] == si) { add_si = 0; }
}
if (add_si) {
sample_indices[i_rand] = si;
i_rand ++;
}
}
// Setup the system:
for (sam = 0; sam < n_samples; sam++) {
A[sam][0] = (float) vectors[sample_indices[sam]].pos.x;
A[sam][1] = (float) vectors[sample_indices[sam]].pos.y;
A[sam][2] = 1.0f;
bu[sam][0] = (float) vectors[sample_indices[sam]].flow_x;
bv[sam][0] = (float) vectors[sample_indices[sam]].flow_y;
//printf("%d,%d,%d,%d,%d\n",A[sam][0],A[sam][1],A[sam][2],bu[sam][0],bv[sam][0]);
}
// Solve the small system:
// for horizontal flow:
// decompose A in u, w, v with singular value decomposition A = u * w * vT.
// u replaces A as output:
pprz_svd_float(A, w, v, n_samples, 3);
pprz_svd_solve_float(pu, A, w, v, bu, n_samples, 3, 1);
PU[it * 3] = pu[0][0];
PU[it * 3 + 1] = pu[1][0];
PU[it * 3 + 2] = pu[2][0];
// for vertical flow:
pprz_svd_solve_float(pv, A, w, v, bv, n_samples, 3, 1);
PV[it * 3] = pv[0][0];
PV[it * 3 + 1] = pv[1][0];
PV[it * 3 + 2] = pv[2][0];
// count inliers and determine their error on all points:
errors_pu[it] = 0;
errors_pv[it] = 0;
n_inliers_pu[it] = 0;
n_inliers_pv[it] = 0;
// for horizontal flow:
// bb = AA * pu:
MAT_MUL(count, 3, 1, bb, AA, pu);
// subtract bu_all: C = 0 in case of perfect fit:
MAT_SUB(count, 1, C, bb, bu_all);
for (p = 0; p < count; p++) {
C[p][0] = abs(C[p][0]);
if (C[p][0] < error_threshold) {
errors_pu[it] += C[p][0];
n_inliers_pu[it]++;
} else {
errors_pu[it] += error_threshold;
}
}
// for vertical flow:
// bb = AA * pv:
MAT_MUL(count, 3, 1, bb, AA, pv);
// subtract bv_all: C = 0 in case of perfect fit:
MAT_SUB(count, 1, C, bb, bv_all);
for (p = 0; p < count; p++) {
C[p][0] = abs(C[p][0]);
if (C[p][0] < error_threshold) {
errors_pv[it] += C[p][0];
n_inliers_pv[it]++;
} else {
errors_pv[it] += error_threshold;
}
}
}
// After all iterations:
// select the parameters with lowest error:
// for horizontal flow:
int param;
int min_ind = 0;
*min_error_u = (float)errors_pu[0];
for (it = 1; it < n_iterations; it++) {
if (errors_pu[it] < *min_error_u) {
*min_error_u = (float)errors_pu[it];
min_ind = it;
}
}
for (param = 0; param < 3; param++) {
parameters_u[param] = PU[min_ind * 3 + param];
}
*n_inliers_u = n_inliers_pu[min_ind];
// for vertical flow:
min_ind = 0;
*min_error_v = (float)errors_pv[0];
for (it = 0; it < n_iterations; it++) {
if (errors_pv[it] < *min_error_v) {
*min_error_v = (float)errors_pv[it];
min_ind = it;
}
}
for (param = 0; param < 3; param++) {
parameters_v[param] = PV[min_ind * 3 + param];
}
*n_inliers_v = n_inliers_pv[min_ind];
// error has to be determined on the entire set without threshold:
// bb = AA * pu:
MAT_MUL(count, 3, 1, bb, AA, pu);
// subtract bu_all: C = 0 in case of perfect fit:
MAT_SUB(count, 1, C, bb, bu_all);
*min_error_u = 0;
for (p = 0; p < count; p++) {
*min_error_u += abs(C[p][0]);
}
// bb = AA * pv:
MAT_MUL(count, 3, 1, bb, AA, pv);
// subtract bv_all: C = 0 in case of perfect fit:
MAT_SUB(count, 1, C, bb, bv_all);
*min_error_v = 0;
for (p = 0; p < count; p++) {
*min_error_v += abs(C[p][0]);
}
*fit_error = (*min_error_u + *min_error_v) / (2 * count);
}