static void
add_unit_direction_vector (double v[2], const double xy1[2],
const double xy2[2], double scale)
{
double xy[2];
vector_sub_nd (xy2, xy1, 2, xy);
vector_normalize_2d (xy);
v[0] += scale * xy[0];
v[1] += scale * xy[1];
}
int
kalman_meas (Kalman * k, const double * z, int M, double dt,
KalmanMeasFunc meas_func, KalmanMeasJacobFunc meas_jacob_func,
KalmanMeasCovFunc meas_cov_func)
{
kalman_pred (k, dt);
double K[k->N * M];
double PHt[k->N * M];
double H[M * k->N];
double R[M * M];
double I[M * M];
double h[M];
gsl_matrix_view Kv = gsl_matrix_view_array (K, k->N, M);
gsl_matrix_view PHtv = gsl_matrix_view_array (PHt, k->N, M);
gsl_matrix_view Hv = gsl_matrix_view_array (H, M, k->N);
gsl_matrix_view Rv = gsl_matrix_view_array (R, M, M);
gsl_matrix_view Iv = gsl_matrix_view_array (I, M, M);
gsl_vector_view hv = gsl_vector_view_array (h, M);
meas_jacob_func (H, M, k->x, k->N, k->user);
/* K = P_*H'*inv(H*P_*H' + R) */
gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, &k->Pv.matrix,
&Hv.matrix, 0.0, &PHtv.matrix);
meas_cov_func (R, M, k->user);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &Hv.matrix,
&PHtv.matrix, 1.0, &Rv.matrix);
size_t permv[M];
gsl_permutation perm = { M, permv };
int signum;
gsl_linalg_LU_decomp (&Rv.matrix, &perm, &signum);
gsl_linalg_LU_invert (&Rv.matrix, &perm, &Iv.matrix);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &PHtv.matrix,
&Iv.matrix, 0.0, &Kv.matrix);
/* x = x + K*(z - h(x)) */
meas_func (h, M, k->x, k->N, k->user);
vector_sub_nd (z, h, M, h);
gsl_blas_dgemv (CblasNoTrans, 1.0, &Kv.matrix, &hv.vector, 1.0,
&k->xv.vector);
/* P = P_ - K*H*P_ */
gsl_blas_dgemm (CblasNoTrans, CblasTrans, -1.0, &Kv.matrix,
&PHtv.matrix, 1.0, &k->Pv.matrix);
return 0;
}
