void mf16_fill_diagonal(mf16 *dest, fix16_t value)
{
int row;
mf16_fill(dest, 0);
for (row = 0; row < dest->rows; row++)
{
dest->data[row][row] = value;
}
}
void mf16_qr_decomposition(mf16 *q, mf16 *r, const mf16 *matrix, int reorthogonalize)
{
int i, j, reorth;
fix16_t dot, norm;
uint8_t stride = FIXMATRIX_MAX_SIZE;
uint8_t n = matrix->rows;
// This uses the modified Gram-Schmidt algorithm.
// subtract_projection takes advantage of the fact that
// previous columns have already been normalized.
// We start with q = matrix
if (q != matrix)
{
*q = *matrix;
}
// R is initialized to have square size of cols(A) and zeroed.
r->columns = matrix->columns;
r->rows = matrix->columns;
r->errors = 0;
mf16_fill(r, 0);
// Now do the actual Gram-Schmidt for the rows.
for (j = 0; j < q->columns; j++)
{
for (reorth = 0; reorth <= reorthogonalize; reorth++)
{
for (i = 0; i < j; i++)
{
fix16_t *v = &q->data[0][j];
fix16_t *u = &q->data[0][i];
dot = fa16_dot(v, stride, u, stride, n);
subtract_projection(v, u, dot, n, &q->errors);
if (dot == fix16_overflow)
q->errors |= FIXMATRIX_OVERFLOW;
r->data[i][j] += dot;
}
}
// Normalize the row in q
norm = fa16_norm(&q->data[0][j], stride, n);
r->data[j][j] = norm;
if (norm == fix16_overflow)
q->errors |= FIXMATRIX_OVERFLOW;
if (norm < 5 && norm > -5)
{
// Nearly zero norm, which means that the row
// was linearly dependent.
q->errors |= FIXMATRIX_SINGULAR;
continue;
}
for (i = 0; i < n; i++)
{
// norm >= v[i] for all i, therefore this division
// doesn't overflow unless norm approaches 0.
q->data[i][j] = fix16_div(q->data[i][j], norm);
}
}
r->errors = q->errors;
}
/*!
* \brief Initializes the gravity Kalman filter
*/
static void kalman_gravity_init()
{
/************************************************************************/
/* initialize the filter structures */
/************************************************************************/
#if USE_UNCONTROLLED
kalman_filter_initialize_uc(&kf, KALMAN_NUM_STATES);
#else
kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS);
#endif
kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS);
/************************************************************************/
/* set initial state */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *x = kalman_get_state_vector_uc(&kf);
#else
mf16 *x = kalman_get_state_vector(&kf);
#endif
x->data[0][0] = 0; // s_i
x->data[1][0] = 0; // v_i
x->data[2][0] = fix16_from_float(6); // g_i
/************************************************************************/
/* set state transition */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *A = kalman_get_state_transition_uc(&kf);
#else
mf16 *A = kalman_get_state_transition(&kf);
#endif
// set time constant
const fix16_t T = fix16_one;
const fix16_t Tsquare = fix16_sq(T);
// helper
const fix16_t fix16_half = fix16_from_float(0.5);
// transition of x to s
matrix_set(A, 0, 0, fix16_one); // 1
matrix_set(A, 0, 1, T); // T
matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2
// transition of x to v
matrix_set(A, 1, 0, 0); // 0
matrix_set(A, 1, 1, fix16_one); // 1
matrix_set(A, 1, 2, T); // T
// transition of x to g
matrix_set(A, 2, 0, 0); // 0
matrix_set(A, 2, 1, 0); // 0
matrix_set(A, 2, 2, fix16_one); // 1
/************************************************************************/
/* set covariance */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *P = kalman_get_system_covariance_uc(&kf);
#else
mf16 *P = kalman_get_system_covariance(&kf);
#endif
matrix_set_symmetric(P, 0, 0, fix16_half); // var(s)
matrix_set_symmetric(P, 0, 1, 0); // cov(s,v)
matrix_set_symmetric(P, 0, 2, 0); // cov(s,g)
matrix_set_symmetric(P, 1, 1, fix16_one); // var(v)
matrix_set_symmetric(P, 1, 2, 0); // cov(v,g)
matrix_set_symmetric(P, 2, 2, fix16_one); // var(g)
/************************************************************************/
/* set system process noise */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *Q = kalman_get_system_process_noise_uc(&kf);
mf16_fill(Q, F16(0.0001));
#endif
/************************************************************************/
/* set measurement transformation */
/************************************************************************/
mf16 *H = kalman_get_observation_transformation(&kfm);
matrix_set(H, 0, 0, fix16_one); // z = 1*s
matrix_set(H, 0, 1, 0); // + 0*v
matrix_set(H, 0, 2, 0); // + 0*g
/************************************************************************/
/* set process noise */
/************************************************************************/
mf16 *R = kalman_get_observation_process_noise(&kfm);
matrix_set(R, 0, 0, fix16_half); // var(s)
}
