amb_kf.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2014 Swift Navigation Inc.
00003  * Contact: Ian Horn <ian@swift-nav.com>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 /*
00014  * This is a Bierman-Thornton kalman filter implementation, as described in:
00015  *  [1] Gibbs, Bruce P. "Advanced Kalman Filtering, Least-Squares, and Modeling."
00016  *      John C. Wiley & Sons, Inc., 2011.
00017  */
00018 
00019 #include <string.h>
00020 #include <stdio.h>
00021 #include <cblas.h>
00022 #include <clapack.h>
00023 // #include <lapacke.h>
00024 #include <math.h>
00025 #include <linear_algebra.h>
00026 #include "constants.h"
00027 #include "track.h"
00028 #include "almanac.h"
00029 #include "gpstime.h"
00030 #include "amb_kf.h"
00031 
00032 #define DEBUG_AMB_KF 0
00033 
00043 void incorporate_scalar_measurement(u32 state_dim, double *h, double R,
00044                                double *U, double *D, double *k)
00045 {
00046   if (DEBUG_AMB_KF) {
00047     printf("<INCORPORATE_SCALAR_MEASUREMENT>\n");
00048     VEC_PRINTF(h, state_dim);
00049     printf("R = %.16f", R);
00050     if (abs(R) == 0) {
00051       printf(" \t (R == 0 exactly)\n");
00052     }
00053     else {
00054       printf("\n");
00055     }
00056   }
00057 
00058   double f[state_dim]; // f = U^T * h
00059   memcpy(f, h, state_dim * sizeof(double));
00060   cblas_dtrmv(CblasRowMajor, CblasUpper, CblasTrans, CblasUnit, //CBLAS_ORDER, CBLAS_UPLO, CBLAS_TRANSPOSE transA, CBLAS_DIAG
00061               state_dim, U, //int N, double *A
00062               state_dim, f, 1); // int lda, double *X, int incX
00063 
00064 
00065   double g[state_dim]; // g = diag(D) * f
00066   double alpha = R; // alpha = f * g + R = f^T * diag(D) * f + R
00067   for (u32 i=0; i<state_dim; i++) {
00068     g[i] = D[i] * f[i];
00069     alpha += f[i] * g[i];
00070   }
00071   if (DEBUG_AMB_KF) {
00072     VEC_PRINTF(f, state_dim);
00073     VEC_PRINTF(g, state_dim);
00074     printf("alpha = %.16f", alpha);
00075     if (abs(alpha) == 0) {
00076       printf(" \t (alpha == 0 exactly)\n");
00077     }
00078     else {
00079       printf("\n");
00080     }
00081   }
00082 
00083   double gamma[state_dim];
00084   double U_bar[state_dim * state_dim];
00085   double D_bar[state_dim];
00086 
00087   memset(gamma, 0,             state_dim * sizeof(double));
00088   memset(U_bar, 0, state_dim * state_dim * sizeof(double));
00089   memset(D_bar, 0,             state_dim * sizeof(double));
00090   memset(k,     0,             state_dim * sizeof(double));
00091 
00092   gamma[0] = R + g[0] * f[0];
00093   if (D[0] == 0 || R == 0) {
00094     D_bar[0] = 0; // this is just an expansion of the other branch with the proper 0 `div` 0 definitions
00095   }
00096   else {
00097     D_bar[0] = D[0] * R / gamma[0];
00098   }
00099   k[0] = g[0];
00100   U_bar[0] = 1;
00101   if (DEBUG_AMB_KF) {
00102     printf("gamma[0] = %f\n", gamma[0]);
00103     printf("D_bar[0] = %f\n", D_bar[0]);
00104     VEC_PRINTF(k, state_dim);
00105     printf("U_bar[:,0] = {");
00106     for (u32 i=0; i < state_dim; i++) {
00107       printf("%f, ", U_bar[i*state_dim]);
00108     }
00109     printf("}\n");
00110   }
00111   for (u32 j=1; j<state_dim; j++) {
00112     gamma[j] = gamma[j-1] + g[j] * f[j];
00113     if (D[j] == 0 || gamma[j-1] == 0) {
00114       D_bar[j] = 0; // this is just an expansion of the other branch with the proper 0 `div` 0 definitions
00115     }
00116     else {
00117       D_bar[j] = D[j] * gamma[j-1] / gamma[j];
00118     }
00119     double f_over_gamma = f[j] / gamma[j-1];
00120     for (u32 i=0; i<=j; i++) {
00121       if (k[i] == 0) {
00122         U_bar[i*state_dim + j] = U[i*state_dim + j]; // same as other branch with correct 0 `div` 0 definitions
00123       }
00124       else {
00125         U_bar[i*state_dim + j] = U[i*state_dim + j] - f_over_gamma * k[i]; // U_bar[:,j] = U[:,j] - f[j]/gamma[j-1] * k
00126       }
00127       k[i] += g[j] * U[i*state_dim + j]; // k = k + g[j] * U[:,j]
00128     }
00129     if (DEBUG_AMB_KF) {
00130       printf("gamma[%"PRIu32"] = %f\n", j, gamma[j]);
00131       printf("D_bar[%"PRIu32"] = %f\n", j, D_bar[j]);
00132       VEC_PRINTF(k, state_dim);
00133       printf("U_bar[:,%"PRIu32"] = {", j);
00134       for (u32 i=0; i < state_dim; i++) {
00135         printf("%f, ", U_bar[i*state_dim + j]);
00136       }
00137       printf("}\n");
00138     }
00139   }
00140   for (u32 i=0; i<state_dim; i++) {
00141     k[i] /= alpha;
00142   }
00143   memcpy(U, U_bar, state_dim * state_dim * sizeof(double));
00144   memcpy(D, D_bar,             state_dim * sizeof(double));
00145   if (DEBUG_AMB_KF) {
00146     MAT_PRINTF(U, state_dim, state_dim);
00147     VEC_PRINTF(D, state_dim);
00148     printf("</INCORPORATE_SCALAR_MEASUREMENT>\n");
00149   }
00150 }
00151 
00155 void incorporate_obs(nkf_t *kf, double *decor_obs)
00156 {
00157   if (DEBUG_AMB_KF) {
00158     printf("<INCORPORATE_OBS>\n");
00159   }
00160   for (u32 i=0; i<kf->obs_dim; i++) {
00161     double *h = &kf->decor_obs_mtx[kf->state_dim * i]; //vector of length kf->state_dim
00162     double R = kf->decor_obs_cov[i]; //scalar
00163     double k[kf->state_dim]; // vector of length kf->state_dim
00164     // printf("i=%i\n", i);
00165     // VEC_PRINTF(h, kf->state_dim);
00166 
00167     incorporate_scalar_measurement(kf->state_dim, h, R, kf->state_cov_U, kf->state_cov_D, &k[0]); //updates cov and sets k
00168     // VEC_PRINTF(k, kf->state_dim);
00169 
00170     double predicted_obs = 0;
00171     for (u32 j=0; j<kf->state_dim; j++) {//TODO take advantage of sparsity of h
00172       predicted_obs += h[j] * kf->state_mean[j];
00173     }
00174     double obs_minus_predicted_obs = decor_obs[i] - predicted_obs;
00175     // printf("decor_obs = %f\n", decor_obs[i]);
00176     // printf("predi_obs = %f\n", predicted_obs);
00177     // printf(" diff_obs = %f\n", obs_minus_predicted_obs);
00178 
00179     for (u32 j=0; j<kf->state_dim; j++) {
00180       kf->state_mean[j] += k[j] * obs_minus_predicted_obs; // uses k to update mean
00181     }
00182     // VEC_PRINTF(intermediate_mean, kf->state_dim);
00183   }
00184   if (DEBUG_AMB_KF) {
00185     printf("</INCORPORATE_OBS>\n");
00186   }
00187 }
00188 
00189 // turns (phi, rho) into Q_tilde * (phi, rho)
00190 void make_residual_measurements(nkf_t *kf, double *measurements, double *resid_measurements)
00191 {
00192   u8 constraint_dim = MAX(kf->state_dim, 3) - 3;
00193   cblas_dgemv (CblasRowMajor, CblasNoTrans, //Order, TransA
00194                constraint_dim, kf->state_dim, // M, N
00195                1, kf->null_basis_Q, kf->state_dim, // alpha A, lda
00196                measurements, 1, // X, incX
00197                0, resid_measurements, 1); // beta, Y, incY
00198   for (u8 i=0; i< kf->state_dim; i++) {
00199     resid_measurements[i+constraint_dim] = measurements[i] - measurements[i+kf->state_dim] / GPS_L1_LAMBDA_NO_VAC;
00200   }
00201 }
00202 
00203 void diffuse_state(nkf_t *kf)
00204 {
00205   for (u8 i=0; i< kf->state_dim; i++) {
00206     kf->state_cov_D[i] += kf->amb_drift_var; //TODO make this a tunable parameter defined at the right time
00207   }
00208 }
00209 
00210 
00213 void nkf_update(nkf_t *kf, double *measurements)
00214 {
00215   if (DEBUG_AMB_KF) {
00216     printf("<NKF_UPDATE>\n");
00217   }
00218   double resid_measurements[kf->obs_dim];
00219   make_residual_measurements(kf, measurements, resid_measurements);
00220   // VEC_PRINTF(measurements, kf->obs_dim);
00221   // MAT_PRINTF(kf->decor_obs_mtx, kf->obs_dim, kf->state_dim);
00222   // MAT_PRINTF(kf->obs_cov_root_inv, kf->obs_dim, kf->obs_dim);
00223 
00224   // replaces residual measurements by their decorrelated version
00225   cblas_dtrmv(CblasRowMajor, CblasUpper, CblasNoTrans, CblasUnit, // Order, Uplo, TransA, Diag
00226               kf->obs_dim, kf->decor_mtx, kf->obs_dim, // N, A, lda
00227               resid_measurements, 1); // X, incX
00228 
00229   // predict_forward(kf);
00230   diffuse_state(kf);
00231   incorporate_obs(kf, resid_measurements);
00232   if (DEBUG_AMB_KF) {
00233     MAT_PRINTF(kf->state_cov_U, kf->state_dim, kf->state_dim);
00234     VEC_PRINTF(kf->state_cov_D, kf->state_dim);
00235     VEC_PRINTF(kf->state_mean, kf->state_dim);
00236     printf("</NKF_UPDATE>\n");
00237   }
00238 }
00239 
00240 // presumes that the first alm entry is the reference sat
00241 void assign_de_mtx(u8 num_sats, sdiff_t *sats_with_ref_first, double ref_ecef[3], double *DE)
00242 {
00243   if (DEBUG_AMB_KF) {
00244     printf("<ASSIGN_DE_MTX>\nnum_sats = %u\nsdiff prns&positions = {\n", num_sats);
00245     for (u8 i=0; i < num_sats; i++) {
00246       printf("i = %u, prn = %u, \tpos = [%f, \t%f, \t%f]\n",
00247              i,
00248              sats_with_ref_first[i].prn,
00249              sats_with_ref_first[i].sat_pos[0],
00250              sats_with_ref_first[i].sat_pos[1],
00251              sats_with_ref_first[i].sat_pos[2]);
00252     }
00253     printf("}\nref_ecef = {%f, \t%f, \t%f}\n", ref_ecef[0], ref_ecef[1], ref_ecef[2]);
00254   }
00255   if (num_sats <= 1) {
00256     if (DEBUG_AMB_KF) {
00257       printf("not enough sats\n</ASSIGN_DE_MTX>\n");
00258     }
00259     return;
00260   }
00261 
00262   memset(DE, 0, (num_sats - 1) * 3 * sizeof(double));
00263   double e0[3];
00264   double x0 = sats_with_ref_first[0].sat_pos[0] - ref_ecef[0];
00265   double y0 = sats_with_ref_first[0].sat_pos[1] - ref_ecef[1];
00266   double z0 = sats_with_ref_first[0].sat_pos[2] - ref_ecef[2];
00267   double norm0 = sqrt(x0*x0 + y0*y0 + z0*z0);
00268   e0[0] = x0 / norm0;
00269   e0[1] = y0 / norm0;
00270   e0[2] = z0 / norm0;
00271   // VEC_PRINTF(ref_ecef, 3);
00272   for (u8 i=1; i<num_sats; i++) {
00273     double x = sats_with_ref_first[i].sat_pos[0] - ref_ecef[0];
00274     double y = sats_with_ref_first[i].sat_pos[1] - ref_ecef[1];
00275     double z = sats_with_ref_first[i].sat_pos[2] - ref_ecef[2];
00276     double norm = sqrt(x*x + y*y + z*z);
00277     DE[3*(i-1)] = x / norm - e0[0];
00278     DE[3*(i-1) + 1] = y / norm - e0[1];
00279     DE[3*(i-1) + 2] = z / norm - e0[2];
00280   }
00281   if (DEBUG_AMB_KF) {
00282     MAT_PRINTF(DE, (num_sats-1), 3);
00283     printf("</ASSIGN_DE_MTX>\n");
00284   }
00285 }
00286 
00287 
00288 /* TODO use the state covariance matrix for a better estimate:
00289  *    That is, decorrelate and scale the LHS of y = A * x before solving for x.
00290  *
00291  *    That is, we assume that y ~ N ( A * x, S), and want to make a maximum
00292  *    likelihood estimate (asymptotically optimal by the Cramer-Rao bound).
00293  *    This is the minimizer of (A * x - y)' * S^-1 * (A * x - y).
00294  *        = min_x  (x' * A' - y') * S^-1 * (A * x - y)
00295  *        = min_x  x' * A' * S^-1* A * x - x' * A' * S^-1 * y
00296  *                                       - y' * S^-1 * A * x  +  y' * S^-1 * y
00297  *    Differentiating by x, this becomes
00298  *    0   =   2 * A' * S^-1 * A * x - 2 * A' * S^-1 * y  ====>
00299  *    A' * S^-1 * y   =   A' * S^-1 * A * x              ====>
00300  *    (A'* S^-1 * A)^-1 * A' * S^-1 * y = x
00301  *
00302  *    Decomposing S^(-1) = U'^-1 * D^-1/2 * D^-1/2 * U^-1 and defining
00303  *    C = D^-1/2 * U^-1 * A, we get
00304  *    (C' * C)^-1 * C' * D^(-1/2) * U^(-1) * y = x
00305  *    We know that (C' * C)^-1 * C' is the pseudoinverse of C for all C, so
00306  *    we're really just solving
00307  *    D^(-1/2) * U^(-1) * y = C * x = D^(-1/2) * U^(-1) * A * x.
00308  *
00309  *
00310  *    Alternatively, we can show this as
00311  *    If y ~ N( A * x, S = U * D * U'),
00312  *        then D^(-1/2) * U^(-1) * y ~  N( D^(-1/2) * U^-1 * A * x, I).
00313  *
00314  *    This form brings more light on the fact that:
00315  *      z = U^(-1) * y ~  N(U^-1 * A * x, D).
00316  *    This form helps us decide how to solve when d_i ~~ 0.
00317  *    The z_I for I = {i | d_i ~~ 0} want to be solved exactly, because we are
00318  *    saying that there is no noise in the measurement. This can reduce the
00319  *    dimension of the problem we are solving, if we really want to.
00320 
00321  *    Therefore we want to:
00322  *      Triangularly solve y = U * z, for z = U^(-1) * y
00323  *                         A = U * B, for C = U^(-1) * A
00324  *        (perhaps simultaneously)
00325  *      Scale w = D^(-1/2) * z, being mindful that D might not be full rank.
00326  *      Scale C = D^(-1/2) * B, being mindful that D might not be full rank.
00327  *      Perform an ordinary least squares solution for w = C * x.
00328  *          -If we have zeros (d_i below some threshold), we could just do an
00329  *              exact solution for those then least squares solve the rest.
00330  *          -Or we could set those d_i to be at threshold and then invert.
00331  *
00332  *    We then have that the covariance matrix for x is (C' * C)^-1.
00333  *
00334  *    We also need to determine if this is even worth the extra effort.
00335  */
00351 void least_squares_solve_b(nkf_t *kf, sdiff_t *sdiffs_with_ref_first,
00352                       double *dd_measurements, double ref_ecef[3], double b[3])
00353 {
00354   if (DEBUG_AMB_KF) {
00355     printf("<LEAST_SQUARES_SOLVE_B>\n");
00356   }
00357   integer num_dds = kf->state_dim;
00358   double DE[num_dds * 3];
00359   assign_de_mtx(num_dds+1, sdiffs_with_ref_first, ref_ecef, DE);
00360   double DET[num_dds * 3];
00361   for (u8 i=0; i<num_dds; i++) { //TODO this transposition is stupid and unnecessary
00362     DET[              i] = DE[i*3 + 0];
00363     DET[    num_dds + i] = DE[i*3 + 1];
00364     DET[2 * num_dds + i] = DE[i*3 + 2];
00365   }
00366 
00367   double phase_ranges[MAX(num_dds,3)];
00368   for (u8 i=0; i< num_dds; i++) {
00369     phase_ranges[i] = dd_measurements[i] - kf->state_mean[i];
00370   }
00371 
00372   if (DEBUG_AMB_KF) {
00373     printf("\tdd_measurements, \tkf->state_mean, \tdifferenced phase_ranges = {\n");
00374     for (u8 i=0; i< num_dds; i++) {
00375       printf("\t%f, \t%f, \t%f,\n", dd_measurements[i], kf->state_mean[i], phase_ranges[i]);
00376     }
00377     printf("\t}\n");
00378   }
00379 
00380   /* TODO could use plain old DGELS here */
00381 
00382   s32 ldb = (s32) MAX(num_dds,3);
00383   double s[3];
00384   double rcond = 1e-12;
00385   s32 rank;
00386   double w[1]; //try 25 + 10*num_sats
00387   s32 lwork = -1;
00388   s32 info;
00389   s32 three = 3;
00390   s32 one = 1;
00391   dgelss_(&num_dds, &three, &one, //M, N, NRHS
00392           DET, &num_dds, //A, LDA
00393           phase_ranges, &ldb, //B, LDB
00394           s, &rcond, // S, RCOND
00395           &rank, //RANK
00396           w, &lwork, // WORK, LWORK
00397           &info); //INFO
00398   lwork = round(w[0]);
00399 
00400   double work[lwork];
00401   dgelss_(&num_dds, &three, &one, //M, N, NRHS
00402           DET, &num_dds, //A, LDA
00403           phase_ranges, &ldb, //B, LDB
00404           s, &rcond, // S, RCOND
00405           &rank, //RANK
00406           work, &lwork, // WORK, LWORK
00407           &info); //INFO
00408   b[0] = phase_ranges[0] * GPS_L1_LAMBDA_NO_VAC;
00409   b[1] = phase_ranges[1] * GPS_L1_LAMBDA_NO_VAC;
00410   b[2] = phase_ranges[2] * GPS_L1_LAMBDA_NO_VAC;
00411   if (DEBUG_AMB_KF) {
00412     printf("b = {%f, %f, %f}\n", b[0]*100, b[1]*100, b[2]*100); // units --> cm
00413     printf("</LEAST_SQUARES_SOLVE_B>\n");
00414   }
00415 }
00416 
00417 
00418 /* presumes that the first alm entry is the reference sat.
00419  *TODO use the state covariance matrix for a better estimate:
00420  *  That is, decorrelate and scale the LHS of y = A * x before solving for x
00421  *TODO consolidate this with the one using the float solution
00422  */
00423 void least_squares_solve_b_external_ambs(u8 num_dds_u8, double *ambs, sdiff_t *sdiffs_with_ref_first, double *dd_measurements, double ref_ecef[3], double b[3])
00424 {
00425   integer num_dds = num_dds_u8;
00426   double DE[num_dds * 3];
00427   assign_de_mtx(num_dds+1, sdiffs_with_ref_first, ref_ecef, DE);
00428   double DET[num_dds * 3];
00429   for (u8 i=0; i<num_dds; i++) { //TODO this transposition is stupid and unnecessary
00430     DET[              i] = DE[i*3 + 0];
00431     DET[    num_dds + i] = DE[i*3 + 1];
00432     DET[2 * num_dds + i] = DE[i*3 + 2];
00433   }
00434 
00435   // double fake_ints[num_dds];
00436   // for (u8 i=0; i< num_dds; i++) {
00437   //   fake_ints[i] = (i+1)*10; //TODO REMOVE THIS version
00438   // }
00439   // double resid[num_dds];
00440   // lesq_solution_b(kf->state_dim, dd_measurements, kf->state_mean, DE, b, resid);
00441   // lesq_solution_b(kf->state_dim, dd_measurements, fake_ints, DE, b, resid);
00442 
00443   double phase_ranges[MAX(num_dds,3)];
00444   for (u8 i=0; i< num_dds; i++) {
00445     phase_ranges[i] = dd_measurements[i] - ambs[i];
00446     // phase_ranges[i] = dd_measurements[i] - (i+1)*10;
00447   }
00448 
00449   //TODO could use plain old DGELS here
00450 
00451   s32 ldb = (s32) MAX(num_dds,3);
00452   double s[3];
00453   double rcond = 1e-12;
00454   s32 rank;
00455   double w[1]; //try 25 + 10*num_sats
00456   s32 lwork = -1;
00457   s32 info;
00458   s32 three = 3;
00459   s32 one = 1;
00460   dgelss_(&num_dds, &three, &one, //M, N, NRHS
00461           DET, &num_dds, //A, LDA
00462           phase_ranges, &ldb, //B, LDB
00463           s, &rcond, // S, RCOND
00464           &rank, //RANK
00465           w, &lwork, // WORK, LWORK
00466           &info); //INFO
00467   lwork = round(w[0]);
00468 
00469   double work[lwork];
00470   dgelss_(&num_dds, &three, &one, //M, N, NRHS
00471           DET, &num_dds, //A, LDA
00472           phase_ranges, &ldb, //B, LDB
00473           s, &rcond, // S, RCOND
00474           &rank, //RANK
00475           work, &lwork, // WORK, LWORK
00476           &info); //INFO
00477 
00478   memcpy(b, phase_ranges, 3 * sizeof(double));
00479   b[0] = phase_ranges[0] * GPS_L1_LAMBDA_NO_VAC;
00480   b[1] = phase_ranges[1] * GPS_L1_LAMBDA_NO_VAC;
00481   b[2] = phase_ranges[2] * GPS_L1_LAMBDA_NO_VAC;
00482 }
00483 
00484 
00485 // initializes the ambiguity means and variances.
00486 // Note that the covariance is  in UDU form, and U starts as identity.
00487 void initialize_state(nkf_t *kf, double *dd_measurements, double init_var)
00488 {
00489   u8 num_dds = kf->state_dim;
00490   for (u32 i=0; i<num_dds; i++) {
00491     // N = Expectation of [phi - rho / lambda]
00492     kf->state_mean[i] = dd_measurements[i] - dd_measurements[i + num_dds] / GPS_L1_LAMBDA_NO_VAC;
00493     // Sigma begins as a diagonal
00494     kf->state_cov_D[i] = init_var;
00495   }
00496   matrix_eye(num_dds, kf->state_cov_U);
00497 }
00498 
00499 void QR_part1(integer m, integer n, double *A, double *tau)
00500 {
00501   double w[1];
00502   integer lwork = -1;
00503   integer info;
00504   integer jpvt[3];
00505   memset(jpvt, 0, 3 * sizeof(integer));
00506   dgeqp3_(&m, &n,
00507           A, &m,
00508           jpvt,
00509           tau,
00510           w, &lwork, &info);
00511   lwork = round(w[0]);
00512   double work[lwork];
00513   dgeqp3_(&m, &n,
00514           A, &m,
00515           jpvt,
00516           tau,
00517           work, &lwork, &info); //set A = QR(A)
00518 }
00519 
00520 void QR_part2(integer m, integer n, double *A, double *tau)
00521 {
00522   double w[1];
00523   integer lwork = -1;
00524   integer info;
00525   dorgqr_(&m, &m, &n,
00526           A, &m,
00527           tau,
00528           w, &lwork, &info);
00529   lwork = round(w[0]);
00530   double work[lwork];
00531   dorgqr_(&m, &m, &n,
00532           A, &m,
00533           tau,
00534           work, &lwork, &info);
00535 }
00536 
00537 void assign_phase_obs_null_basis(u8 num_dds, double *DE_mtx, double *q)
00538 {
00539   // use either GEQRF or GEQP3. GEQP3 is the version with pivoting
00540   // int dgeqrf_(__CLPK_integer *m, __CLPK_integer *n, __CLPK_doublereal *a, __CLPK_integer *
00541   //       lda, __CLPK_doublereal *tau, __CLPK_doublereal *work, __CLPK_integer *lwork, __CLPK_integer *info)
00542   // int dgeqp3_(__CLPK_integer *m, __CLPK_integer *n, __CLPK_doublereal *a, __CLPK_integer *
00543   //       lda, __CLPK_integer *jpvt, __CLPK_doublereal *tau, __CLPK_doublereal *work, __CLPK_integer *lwork,
00544   //        __CLPK_integer *info)
00545 
00546   //DE is num_sats-1 by 3, need to transpose it to column major
00547   double A[num_dds * num_dds];
00548   for (u8 i=0; i < num_dds; i++) {
00549     for (u8 j=0; j<3; j++) {
00550       A[j*num_dds + i] = DE_mtx[i*3 + j]; //set A = Transpose(DE_mtx)
00551     }
00552   }
00553   integer m = num_dds;
00554   integer n = 3;
00555   double tau[3];
00556   QR_part1(m, n, A, tau);
00557   QR_part2(m, n, A, tau);
00558   memcpy(q, &A[3*num_dds], (MAX(3, num_dds) - 3) * num_dds * sizeof(double));
00559 }
00560 
00561 void assign_dd_obs_cov(u8 num_dds, double phase_var, double code_var, double *dd_obs_cov) //TODO this could be made more efficient, if it matters
00562 {
00563   for (u8 i = 0; i < num_dds; i++) {
00564     for (u8 j = 0; j < num_dds; j++) {
00565       if (i == j) {
00566         dd_obs_cov[i*2*num_dds + j] = 2 * phase_var;
00567         dd_obs_cov[(i+num_dds)*2*num_dds + num_dds + j] = 2 * code_var;
00568       }
00569       else {
00570         dd_obs_cov[i*2*num_dds + j] = phase_var;
00571         dd_obs_cov[(i+num_dds)*2*num_dds + num_dds + j] = code_var;
00572       }
00573     }
00574     memset(&dd_obs_cov[i*2*num_dds + num_dds], 0, num_dds * sizeof(double));
00575     memset(&dd_obs_cov[(i+num_dds)*2*num_dds], 0, num_dds * sizeof(double));
00576   }
00577 }
00578 
00579 void assign_residual_obs_cov(u8 num_dds, double phase_var, double code_var, double *q, double *r_cov) //TODO make this more efficient (e.g. via pages 3/6.2-3/2014 of ian's notebook)
00580 {
00581   double dd_obs_cov[4 * num_dds * num_dds];
00582   assign_dd_obs_cov(num_dds, phase_var, code_var, dd_obs_cov);
00583   integer nullspace_dim = MAX(num_dds, 3) - 3;;
00584   integer dd_dim = 2*num_dds;
00585   integer res_dim = num_dds + nullspace_dim;
00586   double q_tilde[res_dim * dd_dim];
00587   memset(q_tilde, 0, res_dim * dd_dim * sizeof(double));
00588   // MAT_PRINTF(obs_cov, dd_dim, dd_dim);
00589 
00590   // MAT_PRINTF(q, nullspace_dim, num_dds);
00591   for (u8 i=0; i<nullspace_dim; i++) {
00592     memcpy(&q_tilde[i*dd_dim], &q[i*num_dds], num_dds * sizeof(double));
00593   }
00594   // MAT_PRINTF(q_tilde, res_dim, dd_dim);
00595   for (u8 i=0; i<num_dds; i++) {
00596     q_tilde[(i+nullspace_dim)*dd_dim + i] = 1;
00597     q_tilde[(i+nullspace_dim)*dd_dim + i+num_dds] = -1 / GPS_L1_LAMBDA_NO_VAC;
00598   }
00599   // MAT_PRINTF(q_tilde, res_dim, dd_dim);
00600 
00601   //TODO make more efficient via the structure of q_tilde, and it's relation to the I + 1*1^T structure of the obs cov mtx
00602   double QC[res_dim * dd_dim];
00603   cblas_dsymm(CblasRowMajor, CblasRight, CblasUpper, //CBLAS_ORDER, CBLAS_SIDE, CBLAS_UPLO
00604               res_dim, dd_dim, // int M, int N
00605               1, dd_obs_cov, dd_dim, // double alpha, double *A, int lda
00606               q_tilde, dd_dim, // double *B, int ldb
00607               0, QC, dd_dim); // double beta, double *C, int ldc
00608   // MAT_PRINTF(QC, res_dim, dd_dim);
00609 
00610   //TODO make more efficient via the structure of q_tilde, and it's relation to the I + 1*1^T structure of the obs cov mtx
00611   cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, // CBLAS_ORDER, CBLAS_TRANSPOSE transA, cBLAS_TRANSPOSE transB
00612               res_dim, res_dim, dd_dim, // int M, int N, int K
00613               1, QC, dd_dim, // double alpha, double *A, int lda
00614               q_tilde, dd_dim, //double *B, int ldb
00615               0, r_cov, res_dim); //beta, double *C, int ldc
00616   // MAT_PRINTF(r_cov_inv, res_dim, res_dim);
00617 }
00618 
00619 void invert_U(u8 res_dim, double *U) // in place inversion of U
00620 {
00621   char uplo = 'U'; //upper triangular
00622   char diag = 'U'; //unit triangular
00623   integer dim = res_dim;
00624   integer lda = res_dim;
00625   integer info;
00626   dtrtri_(&uplo, &diag,
00627           &dim, U, &lda, &info);
00628 }
00629 
00630 void assign_simple_sig(u8 num_dds, double var, double *simple_cov)
00631 {
00632   for (u8 i = 0; i < num_dds; i++) {
00633     for (u8 j = 0; j < num_dds; j++) {
00634       if (i == j) {
00635         simple_cov[i*num_dds + j] = 2 * var;
00636       }
00637       else {
00638         simple_cov[i*num_dds + j] = var;
00639       }
00640     }
00641   }
00642 }
00643 
00644 // from get_kf_matrices:
00645 // U^-1 * y == y'
00646 //           = U^-1 * H * x
00647 //          == H' * x
00648 //
00649 // H = ( Q )
00650 //     ( I )
00651 // where Q's rows form a basis for the left null space for DE
00652 void assign_H_prime(u8 res_dim, u8 constraint_dim, u8 num_dds,
00653                     double *Q, double *U_inv, double *H_prime)
00654 {
00655   // set the H_prime variable to equal H
00656   memcpy(H_prime, Q, constraint_dim * num_dds * sizeof(double));
00657   matrix_eye(num_dds, &H_prime[constraint_dim * num_dds]);
00658 
00659   // multiply H_prime by U_inv to make it the actual H_prime
00660   cblas_dtrmm(CblasRowMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasUnit, // CBLAS_ORDER, CBLAS_SIDE, CBLAS_UPLO, CBLAS_TRANSPOSE, CBLAS_DIAG
00661               res_dim, num_dds, //M, N
00662               1, U_inv, res_dim, //alpha, A, lda
00663               H_prime, num_dds); //B, ldb
00664 }
00665 
00666 // y = H * x
00667 // Var[y] = Sig = U * D * U^T
00668 // ==> Var[U^-1 * y] = D
00669 // U^-1 * y == y'
00670 //           = U^-1 * H * x
00671 //          == H' * x
00672 //
00673 // H = ( Q )
00674 //     ( I )
00675 // where Q's rows form a basis for the left null space for DE
00676 //
00677 // Sig = Q~ * Sig_v * Q~^T
00678 //   where    Q~ = ( Q   0        )
00679 //                 ( I  -I/lambda )
00680 //     and Sig_v = ( D*D^T * var_phi   0               )
00681 //                 ( 0                 D*D^T * var_rho )
00682 //     and D*D^T = 1*1^T + I
00683 //
00684 // This function constructs D, U^-1, and H'
00685 void get_kf_matrices(u8 num_sdiffs, sdiff_t *sdiffs_with_ref_first,
00686                      double ref_ecef[3],
00687                      double phase_var, double code_var,
00688                      double *null_basis_Q,
00689                      double *U_inv, double *D,
00690                      double *H_prime)
00691 {
00692   u8 num_dds = MAX(1, num_sdiffs) - 1;
00693   u8 constraint_dim = MAX(num_dds, 3) - 3;;
00694   u8 res_dim = num_dds + constraint_dim;
00695 
00696   double Sig[res_dim * res_dim];
00697 
00698   //assign Sig and H
00699   if (constraint_dim > 0) {
00700     double DE[num_dds * 3];
00701     assign_de_mtx(num_sdiffs, sdiffs_with_ref_first, ref_ecef, DE);
00702     assign_phase_obs_null_basis(num_dds, DE, null_basis_Q);
00703     assign_residual_obs_cov(num_dds, phase_var, code_var, null_basis_Q, Sig);
00704     //TODO U seems to have that fancy blockwise structure we love so much. Use it
00705     matrix_udu(res_dim, Sig, U_inv, D); //U_inv holds U after this
00706     invert_U(res_dim, U_inv);
00707     //TODO this also has fancy structure
00708     assign_H_prime(res_dim, constraint_dim, num_dds, null_basis_Q, U_inv, H_prime);
00709   }
00710   else {
00711     assign_simple_sig(num_dds,
00712                       phase_var + code_var / (GPS_L1_LAMBDA_NO_VAC * GPS_L1_LAMBDA_NO_VAC),
00713                       Sig);
00714     matrix_udu(res_dim, Sig, U_inv, D); //U_inv holds U after this
00715     invert_U(res_dim, U_inv);
00716 
00717     // H = I in this case, so H' = U^-1 * H = U^-1
00718     memcpy(H_prime, U_inv, num_dds * num_dds * sizeof(double));
00719   }
00720 
00721 }
00722 
00723 
00724 void set_nkf(nkf_t *kf, double amb_drift_var, double phase_var, double code_var, double amb_init_var,
00725             u8 num_sdiffs, sdiff_t *sdiffs_with_ref_first, double *dd_measurements, double ref_ecef[3])
00726 {
00727   if (DEBUG_AMB_KF) {
00728       printf("<SET_NKF>\n");
00729   }
00730   u32 state_dim = num_sdiffs - 1;
00731   u32 num_diffs = num_sdiffs - 1;
00732   kf->state_dim = state_dim;
00733   u32 constraint_dim = MAX(3, num_sdiffs) - 3;
00734   kf->obs_dim = num_diffs + constraint_dim;
00735   kf->amb_drift_var = amb_drift_var;
00736 
00737   get_kf_matrices(num_sdiffs, sdiffs_with_ref_first,
00738                   ref_ecef,
00739                   phase_var, code_var,
00740                   kf->null_basis_Q,
00741                   kf->decor_mtx, kf->decor_obs_cov,
00742                   kf->decor_obs_mtx);
00743 
00744   // given plain old measurements, initialize the state
00745   initialize_state(kf, dd_measurements, amb_init_var);
00746   if (DEBUG_AMB_KF) {
00747       printf("</SET_NKF>\n");
00748   }
00749 }
00750 
00751 void set_nkf_matrices(nkf_t *kf, double phase_var, double code_var,
00752                      u8 num_sdiffs, sdiff_t *sdiffs_with_ref_first, double ref_ecef[3])
00753 {
00754   u32 state_dim = MAX(1, num_sdiffs) - 1;
00755   u32 num_diffs = MAX(1, num_sdiffs) - 1;
00756   kf->state_dim = state_dim;
00757   u32 constraint_dim = MAX(3, num_diffs) - 3;
00758   kf->obs_dim = num_diffs + constraint_dim;
00759 
00760   get_kf_matrices(num_sdiffs, sdiffs_with_ref_first,
00761                   ref_ecef,
00762                   phase_var, code_var,
00763                   kf->null_basis_Q,
00764                   kf->decor_mtx, kf->decor_obs_cov,
00765                   kf->decor_obs_mtx);
00766 }
00767 
00768 s32 find_index_of_element_in_u8s(u32 num_elements, u8 x, u8 *list)
00769 {
00770   for (u32 i=0; i<num_elements; i++) {
00771     if (x == list[i]) {
00772       return i;
00773     }
00774   }
00775   return -1;
00776 }
00777 
00778 void rebase_mean_N(double *mean, u8 num_sats, u8 *old_prns, u8 *new_prns)
00779 {
00780   u8 old_ref = old_prns[0];
00781   u8 new_ref = new_prns[0];
00782 
00783   double new_mean[num_sats-1];
00784   s32 index_of_new_ref_in_old = find_index_of_element_in_u8s(num_sats, new_ref, &old_prns[1]);
00785   double val_for_new_ref_in_old_basis = mean[index_of_new_ref_in_old];
00786   for (u8 i=0; i<num_sats-1; i++) {
00787     u8 new_prn = new_prns[1+i];
00788     if (new_prn == old_ref) {
00789       new_mean[i] = - val_for_new_ref_in_old_basis;
00790     }
00791     else {
00792       s32 index_of_this_sat_in_old_basis = find_index_of_element_in_u8s(num_sats, new_prn, &old_prns[1]);
00793       new_mean[i] = mean[index_of_this_sat_in_old_basis] - val_for_new_ref_in_old_basis;
00794     }
00795   }
00796   memcpy(mean, new_mean, (num_sats-1) * sizeof(double));
00797 }
00798 
00799 void assign_state_rebase_mtx(u8 num_sats, u8 *old_prns, u8 *new_prns, double *rebase_mtx)
00800 {
00801   u8 state_dim = num_sats - 1;
00802   memset(rebase_mtx, 0, state_dim * state_dim * sizeof(double));
00803   u8 old_ref = old_prns[0];
00804   u8 new_ref = new_prns[0];
00805 
00806   s32 index_of_new_ref_in_old = find_index_of_element_in_u8s(num_sats-1, new_ref, &old_prns[1]);
00807   s32 index_of_old_ref_in_new = find_index_of_element_in_u8s(num_sats-1, old_ref, &new_prns[1]);
00808   for (u8 i=0; i<state_dim; i++) {
00809     rebase_mtx[i*state_dim + index_of_new_ref_in_old] = -1;
00810     if (i != (u8) index_of_old_ref_in_new) {
00811       s32 index_of_this_sat_in_old_basis = find_index_of_element_in_u8s(num_sats-1, new_prns[i+1], &old_prns[1]);
00812       rebase_mtx[i*state_dim + index_of_this_sat_in_old_basis] = 1;
00813     }
00814   }
00815   // MAT_PRINTF(rebase_mtx, state_dim, state_dim);
00816 }
00817 
00818 
00819 void rebase_covariance_sigma(double *state_cov, u8 num_sats, u8 *old_prns, u8 *new_prns)
00820 {
00821   u8 state_dim = num_sats - 1;
00822   double rebase_mtx[state_dim * state_dim];
00823   assign_state_rebase_mtx(num_sats, old_prns, new_prns, rebase_mtx);
00824 
00825   double intermediate_cov[state_dim * state_dim];
00826   //TODO make more efficient via structure of rebase_mtx
00827   cblas_dsymm(CblasRowMajor, CblasRight, CblasUpper, //CBLAS_ORDER, CBLAS_SIDE, CBLAS_UPLO
00828               state_dim, state_dim, // int M, int N
00829               1, state_cov, state_dim, // double alpha, double *A, int lda
00830               rebase_mtx, state_dim, // double *B, int ldb
00831               0, intermediate_cov, state_dim); // double beta, double *C, int ldc
00832   // MAT_PRINTF(intermediate_cov, state_dim, state_dim);
00833 
00834   //TODO make more efficient via the structure of rebase_mtx
00835   cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, // CBLAS_ORDER, CBLAS_TRANSPOSE transA, cBLAS_TRANSPOSE transB
00836               state_dim, state_dim, state_dim, // int M, int N, int K
00837               1, intermediate_cov, state_dim, // double alpha, double *A, int lda
00838               rebase_mtx, state_dim, //double *B, int ldb
00839               0, state_cov, state_dim); //beta, double *C, int ldc
00840   // MAT_PRINTF(state_cov, state_dim, state_dim);
00841 }
00842 
00843 void rebase_covariance_udu(double *state_cov_U, double *state_cov_D, u8 num_sats, u8 *old_prns, u8 *new_prns)
00844 {
00845   u8 state_dim = num_sats - 1;
00846   double state_cov[state_dim * state_dim];
00847   matrix_reconstruct_udu(state_dim, state_cov_U, state_cov_D, state_cov);
00848   rebase_covariance_sigma(state_cov, num_sats, old_prns, new_prns);
00849   matrix_udu(state_dim, state_cov, state_cov_U, state_cov_D);
00850 }
00851 
00852 
00853 void rebase_nkf(nkf_t *kf, u8 num_sats, u8 *old_prns, u8 *new_prns)
00854 {
00855   rebase_mean_N(kf->state_mean, num_sats, old_prns, new_prns);
00856   rebase_covariance_udu(kf->state_cov_U, kf->state_cov_D, num_sats, old_prns, new_prns);
00857 }
00858 
00859 void nkf_state_projection(nkf_t *kf,
00860                                     u8 num_old_non_ref_sats,
00861                                     u8 num_new_non_ref_sats,
00862                                     u8 *ndx_of_new_sat_in_old)
00863 {
00864   u8 old_state_dim = num_old_non_ref_sats;
00865   double old_cov[old_state_dim * old_state_dim];
00866   matrix_reconstruct_udu(old_state_dim, kf->state_cov_U, kf->state_cov_D, old_cov);
00867   /*MAT_PRINTF(old_cov, old_state_dim, old_state_dim);*/
00868 
00869   u8 new_state_dim = num_new_non_ref_sats;
00870   double new_cov[new_state_dim * new_state_dim];
00871   double new_mean[new_state_dim];
00872 
00873   for (u8 i=0; i<num_new_non_ref_sats; i++) {
00874     u8 ndxi = ndx_of_new_sat_in_old[i];
00875     new_mean[i] = kf->state_mean[ndxi];
00876     for (u8 j=0; j<num_new_non_ref_sats; j++) {
00877       u8 ndxj = ndx_of_new_sat_in_old[j];
00878       new_cov[i*new_state_dim + j] = old_cov[ndxi*old_state_dim + ndxj];
00879     }
00880   }
00881   /*MAT_PRINTF(new_cov, new_state_dim, new_state_dim);*/
00882 
00883   //put it all back into the kf
00884   memcpy(kf->state_mean, new_mean, new_state_dim * sizeof(double));
00885   matrix_udu(new_state_dim, new_cov, kf->state_cov_U, kf->state_cov_D);
00886   //NOTE: IT DOESN'T UPDATE THE OBSERVATION OR TRANSITION MATRICES, JUST THE STATE
00887 }
00888 
00889 void nkf_state_inclusion(nkf_t *kf,
00890                                    u8 num_old_non_ref_sats,
00891                                    u8 num_new_non_ref_sats,
00892                                    u8 *ndx_of_old_sat_in_new,
00893                                    double int_init_var)
00894 {
00895   u8 old_state_dim = num_old_non_ref_sats;
00896   double old_cov[old_state_dim * old_state_dim];
00897   matrix_reconstruct_udu(old_state_dim, kf->state_cov_U, kf->state_cov_D, old_cov);
00898 
00899   u8 new_state_dim = num_new_non_ref_sats;
00900   double new_cov[new_state_dim * new_state_dim];
00901   memset(new_cov, 0, new_state_dim * new_state_dim * sizeof(double));
00902   double new_mean[new_state_dim];
00903   // initialize_state(kf, dd_measurements, amb_init_var); //TODO do we really want to initialize new states this way?
00904   memset(new_mean, 0, new_state_dim * sizeof(double));
00905   for (u8 i=0; i<num_new_non_ref_sats; i++) {
00906     new_cov[i*new_state_dim + i] = int_init_var;
00907   }
00908 
00909   for (u8 i=0; i<num_old_non_ref_sats; i++) {
00910     u8 ndxi = ndx_of_old_sat_in_new[i];
00911     new_mean[ndxi] = kf->state_mean[i];
00912     for (u8 j=0;j<num_old_non_ref_sats; j++) {
00913       u8 ndxj = ndx_of_old_sat_in_new[j];
00914       new_cov[ndxi*new_state_dim + ndxj] = old_cov[i*old_state_dim + j];
00915     }
00916   }
00917   matrix_udu(new_state_dim, new_cov, kf->state_cov_U, kf->state_cov_D);
00918   memcpy(kf->state_mean, new_mean, new_state_dim * sizeof(double));
00919 }
00920 


swiftnav
Author(s):
autogenerated on Sat Jun 8 2019 18:55:14