00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <string.h>
00020 #include <stdio.h>
00021 #include <cblas.h>
00022 #include <clapack.h>
00023
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];
00059 memcpy(f, h, state_dim * sizeof(double));
00060 cblas_dtrmv(CblasRowMajor, CblasUpper, CblasTrans, CblasUnit,
00061 state_dim, U,
00062 state_dim, f, 1);
00063
00064
00065 double g[state_dim];
00066 double alpha = 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;
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;
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];
00123 }
00124 else {
00125 U_bar[i*state_dim + j] = U[i*state_dim + j] - f_over_gamma * k[i];
00126 }
00127 k[i] += g[j] * U[i*state_dim + 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];
00162 double R = kf->decor_obs_cov[i];
00163 double k[kf->state_dim];
00164
00165
00166
00167 incorporate_scalar_measurement(kf->state_dim, h, R, kf->state_cov_U, kf->state_cov_D, &k[0]);
00168
00169
00170 double predicted_obs = 0;
00171 for (u32 j=0; j<kf->state_dim; j++) {
00172 predicted_obs += h[j] * kf->state_mean[j];
00173 }
00174 double obs_minus_predicted_obs = decor_obs[i] - predicted_obs;
00175
00176
00177
00178
00179 for (u32 j=0; j<kf->state_dim; j++) {
00180 kf->state_mean[j] += k[j] * obs_minus_predicted_obs;
00181 }
00182
00183 }
00184 if (DEBUG_AMB_KF) {
00185 printf("</INCORPORATE_OBS>\n");
00186 }
00187 }
00188
00189
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,
00194 constraint_dim, kf->state_dim,
00195 1, kf->null_basis_Q, kf->state_dim,
00196 measurements, 1,
00197 0, resid_measurements, 1);
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;
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
00221
00222
00223
00224
00225 cblas_dtrmv(CblasRowMajor, CblasUpper, CblasNoTrans, CblasUnit,
00226 kf->obs_dim, kf->decor_mtx, kf->obs_dim,
00227 resid_measurements, 1);
00228
00229
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
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
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
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
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++) {
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
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];
00387 s32 lwork = -1;
00388 s32 info;
00389 s32 three = 3;
00390 s32 one = 1;
00391 dgelss_(&num_dds, &three, &one,
00392 DET, &num_dds,
00393 phase_ranges, &ldb,
00394 s, &rcond,
00395 &rank,
00396 w, &lwork,
00397 &info);
00398 lwork = round(w[0]);
00399
00400 double work[lwork];
00401 dgelss_(&num_dds, &three, &one,
00402 DET, &num_dds,
00403 phase_ranges, &ldb,
00404 s, &rcond,
00405 &rank,
00406 work, &lwork,
00407 &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);
00413 printf("</LEAST_SQUARES_SOLVE_B>\n");
00414 }
00415 }
00416
00417
00418
00419
00420
00421
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++) {
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
00436
00437
00438
00439
00440
00441
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
00447 }
00448
00449
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];
00456 s32 lwork = -1;
00457 s32 info;
00458 s32 three = 3;
00459 s32 one = 1;
00460 dgelss_(&num_dds, &three, &one,
00461 DET, &num_dds,
00462 phase_ranges, &ldb,
00463 s, &rcond,
00464 &rank,
00465 w, &lwork,
00466 &info);
00467 lwork = round(w[0]);
00468
00469 double work[lwork];
00470 dgelss_(&num_dds, &three, &one,
00471 DET, &num_dds,
00472 phase_ranges, &ldb,
00473 s, &rcond,
00474 &rank,
00475 work, &lwork,
00476 &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
00486
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
00492 kf->state_mean[i] = dd_measurements[i] - dd_measurements[i + num_dds] / GPS_L1_LAMBDA_NO_VAC;
00493
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);
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
00540
00541
00542
00543
00544
00545
00546
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];
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)
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)
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
00589
00590
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
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
00600
00601
00602 double QC[res_dim * dd_dim];
00603 cblas_dsymm(CblasRowMajor, CblasRight, CblasUpper,
00604 res_dim, dd_dim,
00605 1, dd_obs_cov, dd_dim,
00606 q_tilde, dd_dim,
00607 0, QC, dd_dim);
00608
00609
00610
00611 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
00612 res_dim, res_dim, dd_dim,
00613 1, QC, dd_dim,
00614 q_tilde, dd_dim,
00615 0, r_cov, res_dim);
00616
00617 }
00618
00619 void invert_U(u8 res_dim, double *U)
00620 {
00621 char uplo = 'U';
00622 char diag = 'U';
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
00645
00646
00647
00648
00649
00650
00651
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
00656 memcpy(H_prime, Q, constraint_dim * num_dds * sizeof(double));
00657 matrix_eye(num_dds, &H_prime[constraint_dim * num_dds]);
00658
00659
00660 cblas_dtrmm(CblasRowMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasUnit,
00661 res_dim, num_dds,
00662 1, U_inv, res_dim,
00663 H_prime, num_dds);
00664 }
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
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
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
00705 matrix_udu(res_dim, Sig, U_inv, D);
00706 invert_U(res_dim, U_inv);
00707
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);
00715 invert_U(res_dim, U_inv);
00716
00717
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
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
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
00827 cblas_dsymm(CblasRowMajor, CblasRight, CblasUpper,
00828 state_dim, state_dim,
00829 1, state_cov, state_dim,
00830 rebase_mtx, state_dim,
00831 0, intermediate_cov, state_dim);
00832
00833
00834
00835 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
00836 state_dim, state_dim, state_dim,
00837 1, intermediate_cov, state_dim,
00838 rebase_mtx, state_dim,
00839 0, state_cov, state_dim);
00840
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
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
00882
00883
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
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
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