swiftnav: amb_kf.h Source File

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 #ifndef LIBSWIFTNAV_AMB_KF_H
00014 #define LIBSWIFTNAV_AMB_KF_H
00015 #include "track.h"
00016 #include "almanac.h"
00017 #include "gpstime.h"
00018 #include "common.h"
00019 #include "single_diff.h"
00020 #include "constants.h"
00021 
00022 #define MAX_STATE_DIM (MAX_CHANNELS - 1)
00023 #define MAX_OBS_DIM (2 * MAX_CHANNELS - 5)
00024 
00025 typedef struct {
00026   u32 state_dim;
00027   u32 obs_dim;
00028   double amb_drift_var;
00029   double decor_mtx[MAX_OBS_DIM * MAX_OBS_DIM]; //the decorrelation matrix. takes raw measurements and decorrelates them
00030   double decor_obs_mtx[MAX_STATE_DIM * MAX_OBS_DIM]; //the observation matrix for decorrelated measurements
00031   double decor_obs_cov[MAX_OBS_DIM]; //the diagonal of the decorrelated observation covariance (for cholesky is ones)
00032   double null_basis_Q[(MAX_STATE_DIM - 3) * MAX_OBS_DIM];
00033   double state_mean[MAX_STATE_DIM];
00034   double state_cov_U[MAX_STATE_DIM * MAX_STATE_DIM];
00035   double state_cov_D[MAX_STATE_DIM];
00036 } nkf_t;
00037 
00038 // void predict_forward(nkf_t *kf);
00039 void nkf_update(nkf_t *kf, double *measurements);
00040 
00041 void assign_de_mtx(u8 num_sats, sdiff_t *sats_with_ref_first, double ref_ecef[3], double *DE);
00042 
00043 void assign_phase_obs_null_basis(u8 num_dds, double *DE_mtx, double *q);
00044 void set_nkf(nkf_t *kf, double amb_drift_var, double phase_var, double code_var, double amb_init_var,
00045             u8 num_sdiffs, sdiff_t *sdiffs_with_ref_first, double *dd_measurements, double ref_ecef[3]);
00046 void set_nkf_matrices(nkf_t *kf, double phase_var, double code_var,
00047                      u8 num_sdiffs, sdiff_t *sdiffs_with_ref_first, double ref_ecef[3]);
00048 s32 find_index_of_element_in_u8s(u32 num_elements, u8 x, u8 *list);
00049 void rebase_nkf(nkf_t *kf, u8 num_sats, u8 *old_prns, u8 *new_prns);
00050 
00051 void nkf_state_projection(nkf_t *kf,
00052                                     u8 num_old_non_ref_sats,
00053                                     u8 num_new_non_ref_sats,
00054                                     u8 *ndx_of_new_sat_in_old);
00055 void nkf_state_inclusion(nkf_t *kf,
00056                                    u8 num_old_non_ref_sats,
00057                                    u8 num_new_non_ref_sats,
00058                                    u8 *ndx_of_old_sat_in_new,
00059                                    double int_init_var);
00060 
00061 void rebase_nkf(nkf_t *kf, u8 num_sats, u8 *old_prns, u8 *new_prns);
00062 void rebase_covariance_udu(double *state_cov_U, double *state_cov_D, u8 num_sats, u8 *old_prns, u8 *new_prns);
00063 void least_squares_solve_b(nkf_t *kf, sdiff_t *sdiffs_with_ref_first, double *dd_measurements, double ref_ecef[3], double b[3]);
00064 void least_squares_solve_b_external_ambs(u8 num_dds, double *ambs, sdiff_t *sdiffs_with_ref_first, double *dd_measurements, double ref_ecef[3], double b[3]);
00065 
00066 void rebase_mean_N(double *mean, u8 num_sats, u8 *old_prns, u8 *new_prns);
00067 void rebase_covariance_sigma(double *state_cov, u8 num_sats, u8 *old_prns, u8 *new_prns);
00068 
00069 #endif /* LIBSWIFTNAV_AMBFLOAT_KF_H */
00070