1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-16 10:09:58 +00:00

Merge branch 'kf19-next' of https://github.com/glamountain/gnss-sdr into glamountain-kf19-next

This commit is contained in:
Carles Fernandez 2019-06-13 17:14:03 +02:00
commit ce3d018f67
7 changed files with 437 additions and 3 deletions

View File

@ -33,6 +33,7 @@ set(TRACKING_LIB_SOURCES
cpu_multicorrelator.cc
cpu_multicorrelator_real_codes.cc
cpu_multicorrelator_16sc.cc
cubature_filter.cc
lock_detectors.cc
tcp_communication.cc
tcp_packet_data.cc
@ -50,6 +51,7 @@ set(TRACKING_LIB_HEADERS
cpu_multicorrelator.h
cpu_multicorrelator_real_codes.h
cpu_multicorrelator_16sc.h
cubature_filter.h
lock_detectors.h
tcp_communication.h
tcp_packet_data.h

View File

@ -0,0 +1,190 @@
/*!
* \file cubature_filter.cc
* \brief Interface of a library with Bayesian noise statistic estimation
*
* Cubature_Filter implements the functionality of the Cubature Kalman
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system.
*
* [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
* Transactions on Automatic Control, 54(6):12541269,2009.
*
* \authors <ul>
* <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
* <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
* </ul>
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "cubature_filter.h"
Cubature_filter::Cubature_filter()
{
int nx = 1;
x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
x_est = x_pred_out;
P_x_est = P_x_pred_out;
}
Cubature_filter::Cubature_filter(int nx)
{
x_pred_out = arma::zeros(nx, 1);
P_x_pred_out = arma::eye(nx, nx) * (nx + 1);
x_est = x_pred_out;
P_x_est = P_x_pred_out;
}
Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
P_x_pred_out = P_x_pred_0;
x_est = x_pred_out;
P_x_est = P_x_pred_out;
}
Cubature_filter::~Cubature_filter() = default;
void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0)
{
x_pred_out = x_pred_0;
P_x_pred_out = P_x_pred_0;
x_est = x_pred_out;
P_x_est = P_x_pred_out;
}
/*
* Perform the prediction step of the cubature Kalman filter
*/
void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance)
{
// Compute number of cubature points
int nx = x_post.n_elem;
int np = 2 * nx;
// Generator Matrix
arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx));
// Initialize predicted mean and covariance
arma::vec x_pred = arma::zeros(nx,1);
arma::mat P_x_pred = arma::zeros(nx,nx);
// Factorize posterior covariance
arma::mat Sm_post = arma::chol(P_x_post, "lower");
// Propagate and evaluate cubature points
arma::vec Xi_post;
arma::vec Xi_pred;
for (uint8_t i = 0; i < np; i++)
{
Xi_post = Sm_post * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_post;
Xi_pred = (*transition_fcn)(Xi_post);
x_pred = x_pred + Xi_pred;
P_x_pred = P_x_pred + Xi_pred*Xi_pred.t();
}
// Estimate predicted state and error covariance
x_pred = x_pred / ((float) np);
P_x_pred = P_x_pred / ((float) np) - x_pred*x_pred.t() + noise_covariance;
// Store predicted state and error covariance
x_pred_out = x_pred;
P_x_pred_out = P_x_pred;
}
/*
* Perform the update step of the cubature Kalman filter
*/
void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance)
{
// Compute number of cubature points
int nx = x_pred.n_elem;
int nz = z_upd.n_elem;
int np = 2 * nx;
// Generator Matrix
arma::mat gen_one = arma::join_horiz(arma::eye(nx,nx),-1.0*arma::eye(nx,nx));
// Evaluate predicted measurement and covariances
arma::mat z_pred = arma::zeros(nz,1);
arma::mat P_zz_pred = arma::zeros(nz,nz);
arma::mat P_xz_pred = arma::zeros(nx,nz);
// Factorize predicted covariance
arma::mat Sm_pred = arma::chol(P_x_pred, "lower");
// Propagate and evaluate cubature points
arma::vec Xi_pred;
arma::vec Zi_pred;
for (uint8_t i = 0; i < np; i++)
{
Xi_pred = Sm_pred * (std::sqrt(((float) np) / 2.0) * gen_one.col(i)) + x_pred;
Zi_pred = (*measurement_fcn)(Xi_pred);
z_pred = z_pred + Zi_pred;
P_zz_pred = P_zz_pred + Zi_pred*Zi_pred.t();
P_xz_pred = P_xz_pred + Xi_pred*Zi_pred.t();
}
// Estimate measurement covariance and cross covariances
z_pred = z_pred / ((float) np);
P_zz_pred = P_zz_pred / ((float) np) - z_pred*z_pred.t() + noise_covariance;
P_xz_pred = P_xz_pred / ((float) np) - x_pred*z_pred.t();
// Estimate cubature Kalman gain
arma::mat W_k = P_xz_pred*arma::inv(P_zz_pred);
// Estimate and store the updated state and error covariance
x_est = x_pred + W_k*(z_upd - z_pred);
P_x_est = P_x_pred - W_k*P_zz_pred*W_k.t();
}
arma::mat Cubature_filter::get_x_pred() const
{
return x_pred_out;
}
arma::mat Cubature_filter::get_P_x_pred() const
{
return P_x_pred_out;
}
arma::mat Cubature_filter::get_x_est() const
{
return x_est;
}
arma::mat Cubature_filter::get_P_x_est() const
{
return P_x_est;
}

View File

@ -0,0 +1,84 @@
/*!
* \file cubature_filter.h
* \brief Interface of a library with Bayesian noise statistic estimation
*
* Cubature_Filter implements the functionality of the Cubature Kalman
* Filter, which uses multidimensional cubature rules to estimate the
* time evolution of a nonlinear system.
*
* [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
* Transactions on Automatic Control, 54(6):12541269,2009.
*
* \authors <ul>
* <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
* <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
* </ul>
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_CUBATURE_FILTER_H_
#define GNSS_SDR_CUBATURE_FILTER_H_
#include <armadillo>
#include <gnuradio/gr_complex.h>
// Abstract model function
class Model_Function{
public:
Model_Function() {};
virtual arma::vec operator() (arma::vec input) = 0;
virtual ~Model_Function() = default;
};
class Cubature_filter
{
public:
// Constructors and destructors
Cubature_filter();
Cubature_filter(int nx);
Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
~Cubature_filter();
// Reinitialization function
void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0);
// Prediction and estimation
void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance);
void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance);
// Getters
arma::mat get_x_pred() const;
arma::mat get_P_x_pred() const;
arma::mat get_x_est() const;
arma::mat get_P_x_est() const;
private:
arma::vec x_pred_out;
arma::mat P_x_pred_out;
arma::vec x_est;
arma::mat P_x_est;
};
#endif

View File

@ -799,6 +799,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA)
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc
)
if(${FILESYSTEM_FOUND})
target_compile_definitions(trk_test PRIVATE -DHAS_STD_FILESYSTEM=1)

View File

@ -99,6 +99,7 @@ DECLARE_string(log_dir);
#endif
#include "unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc"

View File

@ -1,7 +1,7 @@
/*!
* \file bayesian_estimation_positivity_test.cc
* \brief This file implements timing tests for the Bayesian covariance estimator
* \author Gerald LaMountain, 20168. gerald(at)ece.neu.edu
* \file bayesian_estimation_test.cc
* \brief This file implements feasability test for the BCE library.
* \author Gerald LaMountain, 2018. gerald(at)ece.neu.edu
*
*
* -------------------------------------------------------------------------

View File

@ -0,0 +1,156 @@
/*!
* \file cubature_filter_test.cc
* \brief This file implements numerical accuracy test for the CKF library.
* \author Gerald LaMountain, 2019. gerald(at)ece.neu.edu
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "cubature_filter.h"
#include <armadillo>
#include <gtest/gtest.h>
#include <random>
#define CUBATURE_TEST_N_TRIALS 1000
class Transition_Model : public Model_Function {
public:
Transition_Model(arma::mat kf_F) {coeff_mat = kf_F;};
virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;};
private:
arma::mat coeff_mat;
};
class Measurement_Model : public Model_Function {
public:
Measurement_Model(arma::mat kf_H) {coeff_mat = kf_H;};
virtual arma::vec operator() (arma::vec input) {return coeff_mat*input;};
private:
arma::mat coeff_mat;
};
TEST(CubatureFilterComputationTest, CubatureFilterTest)
{
Cubature_filter kf_cubature;
arma::vec kf_x;
arma::mat kf_P_x;
arma::vec kf_x_pre;
arma::mat kf_P_x_pre;
arma::vec ckf_x_pre;
arma::mat ckf_P_x_pre;
arma::vec kf_x_post;
arma::mat kf_P_x_post;
arma::vec ckf_x_post;
arma::mat ckf_P_x_post;
arma::mat kf_F;
arma::mat kf_H;
arma::mat kf_Q;
arma::mat kf_R;
arma::vec eta;
arma::vec nu;
arma::vec kf_y;
arma::mat kf_P_y;
arma::mat kf_K;
Model_Function* transition_function;
Model_Function* measurement_function;
//--- Perform initializations ------------------------------
std::random_device r;
std::default_random_engine e1(r());
std::normal_distribution<float> normal_dist(0, 5);
std::uniform_real_distribution<float> uniform_dist(0.1, 5.0);
uint8_t nx = 0;
uint8_t ny = 0;
for (uint16_t k = 0; k < CUBATURE_TEST_N_TRIALS; k++)
{
nx = std::rand() % 5 + 1;
ny = std::rand() % 5 + 1;
kf_x = arma::randn<arma::vec>(nx,1);
kf_P_x_post = 5.0 * arma::diagmat(arma::randu<arma::vec>(nx,1));
kf_x_post = arma::mvnrnd(kf_x, kf_P_x_post);
kf_cubature.initialize(kf_x_post, kf_P_x_post);
// Prediction Step
kf_F = arma::randu<arma::mat>(nx,nx);
kf_Q = arma::diagmat(arma::randu<arma::vec>(nx,1));
transition_function = new Transition_Model(kf_F);
arma::mat ttx = (*transition_function)(kf_x_post);
kf_cubature.predict_sequential(kf_x_post,kf_P_x_post,transition_function,kf_Q);
ckf_x_pre = kf_cubature.get_x_pred();
ckf_P_x_pre = kf_cubature.get_P_x_pred();
kf_x_pre = kf_F * kf_x_post;
kf_P_x_pre = kf_F * kf_P_x_post * kf_F.t() + kf_Q;
EXPECT_TRUE(arma::approx_equal(ckf_x_pre, kf_x_pre, "absdiff", 0.01));
EXPECT_TRUE(arma::approx_equal(ckf_P_x_pre, kf_P_x_pre, "absdiff", 0.01));
// Update Step
kf_H = arma::randu<arma::mat>(ny,nx);
kf_R = arma::diagmat(arma::randu<arma::vec>(ny,1));
eta = arma::mvnrnd(arma::zeros<arma::vec>(nx,1),kf_Q);
nu = arma::mvnrnd(arma::zeros<arma::vec>(ny,1),kf_R);
kf_y = kf_H*(kf_F*kf_x + eta) + nu;
measurement_function = new Measurement_Model(kf_H);
kf_cubature.update_sequential(kf_y,kf_x_pre,kf_P_x_pre,measurement_function,kf_R);
ckf_x_post = kf_cubature.get_x_est();
ckf_P_x_post = kf_cubature.get_P_x_est();
kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R;
kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y);
kf_x_post = kf_x_pre + kf_K * (kf_y - kf_H * kf_x_pre);
kf_P_x_post = (arma::eye(nx,nx) - kf_K * kf_H) * kf_P_x_pre;
EXPECT_TRUE(arma::approx_equal(ckf_x_post, kf_x_post, "absdiff", 0.01));
EXPECT_TRUE(arma::approx_equal(ckf_P_x_post, kf_P_x_post, "absdiff", 0.01));
delete transition_function;
delete measurement_function;
}
}