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; }