mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-11-04 09:13:05 +00:00
Merge branch 'glamountain-kf19-next' into next
This commit is contained in:
Carles Fernandez
@@ -33,7 +33,7 @@ set(TRACKING_LIB_SOURCES
|
||||
cpu_multicorrelator.cc
|
||||
cpu_multicorrelator_real_codes.cc
|
||||
cpu_multicorrelator_16sc.cc
|
||||
cubature_filter.cc
|
||||
nonlinear_tracking.cc
|
||||
lock_detectors.cc
|
||||
tcp_communication.cc
|
||||
tcp_packet_data.cc
|
||||
@@ -51,7 +51,7 @@ set(TRACKING_LIB_HEADERS
|
||||
cpu_multicorrelator.h
|
||||
cpu_multicorrelator_real_codes.h
|
||||
cpu_multicorrelator_16sc.h
|
||||
cubature_filter.h
|
||||
nonlinear_tracking.h
|
||||
lock_detectors.h
|
||||
tcp_communication.h
|
||||
tcp_packet_data.h
|
||||
|
||||
@@ -1,199 +0,0 @@
|
||||
/*!
|
||||
* \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):1254–1269,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(static_cast<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 / static_cast<float>(np);
|
||||
P_x_pred = P_x_pred / static_cast<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(static_cast<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 / static_cast<float>(np);
|
||||
P_zz_pred = P_zz_pred / static_cast<float>(np) - z_pred * z_pred.t() + noise_covariance;
|
||||
P_xz_pred = P_xz_pred / static_cast<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;
|
||||
}
|
||||
390
src/algorithms/tracking/libs/nonlinear_tracking.cc
Normal file
390
src/algorithms/tracking/libs/nonlinear_tracking.cc
Normal file
@@ -0,0 +1,390 @@
|
||||
/*!
|
||||
* \file cubature_filter.cc
|
||||
* \brief Interface of a library for nonlinear tracking algorithms
|
||||
*
|
||||
* Cubature_Filter implements the functionality of the Cubature Kalman
|
||||
* Filter, which uses multidimensional cubature rules to estimate the
|
||||
* time evolution of a nonlinear system. Unscented_filter implements
|
||||
* an Unscented Kalman Filter which uses Unscented Transform rules to
|
||||
* perform a similar estimation.
|
||||
*
|
||||
* [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
|
||||
* Transactions on Automatic Control, 54(6):1254–1269,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 "nonlinear_tracking.h"
|
||||
|
||||
/***************** CUBATURE KALMAN FILTER *****************/
|
||||
|
||||
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(static_cast<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();
|
||||
}
|
||||
|
||||
// Compute predicted mean and error covariance
|
||||
x_pred = x_pred / static_cast<float>(np);
|
||||
P_x_pred = P_x_pred / static_cast<float>(np) - x_pred * x_pred.t() + noise_covariance;
|
||||
|
||||
// Store predicted mean 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));
|
||||
|
||||
// Initialize estimated 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(static_cast<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();
|
||||
}
|
||||
|
||||
// Compute measurement mean, covariance and cross covariance
|
||||
z_pred = z_pred / static_cast<float>(np);
|
||||
P_zz_pred = P_zz_pred / static_cast<float>(np) - z_pred * z_pred.t() + noise_covariance;
|
||||
P_xz_pred = P_xz_pred / static_cast<float>(np) - x_pred * z_pred.t();
|
||||
|
||||
// Compute cubature Kalman gain
|
||||
arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred);
|
||||
|
||||
// Compute and store the updated mean 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;
|
||||
}
|
||||
/***************** END CUBATURE KALMAN FILTER *****************/
|
||||
|
||||
|
||||
/***************** UNSCENTED KALMAN FILTER *****************/
|
||||
|
||||
Unscented_filter::Unscented_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;
|
||||
}
|
||||
|
||||
|
||||
Unscented_filter::Unscented_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;
|
||||
}
|
||||
|
||||
|
||||
Unscented_filter::Unscented_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;
|
||||
}
|
||||
|
||||
|
||||
Unscented_filter::~Unscented_filter() = default;
|
||||
|
||||
|
||||
void Unscented_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 Unscented Kalman filter
|
||||
*/
|
||||
void Unscented_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 sigma points
|
||||
int nx = x_post.n_elem;
|
||||
int np = 2 * nx + 1;
|
||||
|
||||
float alpha = 0.001;
|
||||
float kappa = 0.0;
|
||||
float beta = 2.0;
|
||||
|
||||
float lambda = std::pow(alpha, 2.0) * (static_cast<float>(nx) + kappa) - static_cast<float>(nx);
|
||||
|
||||
// Compute UT Weights
|
||||
float W0_m = lambda / (static_cast<float>(nx) + lambda);
|
||||
float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1 - std::pow(alpha, 2.0) + beta);
|
||||
float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
|
||||
|
||||
// Propagate and evaluate sigma points
|
||||
arma::mat Xi_fact = arma::zeros(nx, nx);
|
||||
arma::mat Xi_post = arma::zeros(nx, np);
|
||||
arma::mat Xi_pred = arma::zeros(nx, np);
|
||||
|
||||
|
||||
Xi_post.col(0) = x_post;
|
||||
Xi_pred.col(0) = (*transition_fcn)(Xi_post.col(0));
|
||||
for (uint8_t i = 1; i <= nx; i++)
|
||||
{
|
||||
Xi_fact = std::sqrt(static_cast<float>(nx) + lambda) * arma::sqrtmat_sympd(P_x_post);
|
||||
Xi_post.col(i) = x_post + Xi_fact.col(i - 1);
|
||||
Xi_post.col(i + nx) = x_post - Xi_fact.col(i - 1);
|
||||
|
||||
Xi_pred.col(i) = (*transition_fcn)(Xi_post.col(i));
|
||||
Xi_pred.col(i + nx) = (*transition_fcn)(Xi_post.col(i + nx));
|
||||
}
|
||||
|
||||
// Compute predicted mean
|
||||
arma::vec x_pred = W0_m * Xi_pred.col(0) + Wi_m * arma::sum(Xi_pred.cols(1, np - 1), 1);
|
||||
|
||||
// Compute predicted error covariance
|
||||
arma::mat P_x_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Xi_pred.col(0).t() - x_pred.t()));
|
||||
for (uint8_t i = 1; i < np; i++)
|
||||
{
|
||||
P_x_pred = P_x_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Xi_pred.col(i).t() - x_pred.t()));
|
||||
}
|
||||
P_x_pred = P_x_pred + noise_covariance;
|
||||
|
||||
// Store predicted mean and error covariance
|
||||
x_pred_out = x_pred;
|
||||
P_x_pred_out = P_x_pred;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Perform the update step of the Unscented Kalman filter
|
||||
*/
|
||||
void Unscented_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 sigma points
|
||||
int nx = x_pred.n_elem;
|
||||
int nz = z_upd.n_elem;
|
||||
int np = 2 * nx + 1;
|
||||
|
||||
float alpha = 0.001;
|
||||
float kappa = 0.0;
|
||||
float beta = 2.0;
|
||||
|
||||
float lambda = std::pow(alpha, 2.0) * (static_cast<float>(nx) + kappa) - static_cast<float>(nx);
|
||||
|
||||
// Compute UT Weights
|
||||
float W0_m = lambda / (static_cast<float>(nx) + lambda);
|
||||
float W0_c = lambda / (static_cast<float>(nx) + lambda) + (1.0 - std::pow(alpha, 2.0) + beta);
|
||||
float Wi_m = 1.0 / (2.0 * (static_cast<float>(nx) + lambda));
|
||||
|
||||
// Propagate and evaluate sigma points
|
||||
arma::mat Xi_fact = arma::zeros(nx, nx);
|
||||
arma::mat Xi_pred = arma::zeros(nx, np);
|
||||
arma::mat Zi_pred = arma::zeros(nz, np);
|
||||
|
||||
Xi_pred.col(0) = x_pred;
|
||||
Zi_pred.col(0) = (*measurement_fcn)(Xi_pred.col(0));
|
||||
for (uint8_t i = 1; i <= nx; i++)
|
||||
{
|
||||
Xi_fact = std::sqrt(static_cast<float>(nx) + lambda) * arma::sqrtmat_sympd(P_x_pred);
|
||||
Xi_pred.col(i) = x_pred + Xi_fact.col(i - 1);
|
||||
Xi_pred.col(i + nx) = x_pred - Xi_fact.col(i - 1);
|
||||
|
||||
Zi_pred.col(i) = (*measurement_fcn)(Xi_pred.col(i));
|
||||
Zi_pred.col(i + nx) = (*measurement_fcn)(Xi_pred.col(i + nx));
|
||||
}
|
||||
|
||||
// Compute measurement mean
|
||||
arma::mat z_pred = W0_m * Zi_pred.col(0) + Wi_m * arma::sum(Zi_pred.cols(1, np - 1), 1);
|
||||
|
||||
// Compute measurement covariance and cross covariance
|
||||
arma::mat P_zz_pred = W0_c * ((Zi_pred.col(0) - z_pred) * (Zi_pred.col(0).t() - z_pred.t()));
|
||||
arma::mat P_xz_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Zi_pred.col(0).t() - z_pred.t()));
|
||||
for (uint8_t i = 0; i < np; i++)
|
||||
{
|
||||
P_zz_pred = P_zz_pred + Wi_m * ((Zi_pred.col(i) - z_pred) * (Zi_pred.col(i).t() - z_pred.t()));
|
||||
P_xz_pred = P_xz_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Zi_pred.col(i).t() - z_pred.t()));
|
||||
}
|
||||
P_zz_pred = P_zz_pred + noise_covariance;
|
||||
|
||||
// Estimate cubature Kalman gain
|
||||
arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred);
|
||||
|
||||
// Estimate and store the updated mean 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 Unscented_filter::get_x_pred() const
|
||||
{
|
||||
return x_pred_out;
|
||||
}
|
||||
|
||||
|
||||
arma::mat Unscented_filter::get_P_x_pred() const
|
||||
{
|
||||
return P_x_pred_out;
|
||||
}
|
||||
|
||||
|
||||
arma::mat Unscented_filter::get_x_est() const
|
||||
{
|
||||
return x_est;
|
||||
}
|
||||
|
||||
|
||||
arma::mat Unscented_filter::get_P_x_est() const
|
||||
{
|
||||
return P_x_est;
|
||||
}
|
||||
|
||||
/***************** END UNSCENTED KALMAN FILTER *****************/
|
||||
@@ -1,10 +1,12 @@
|
||||
/*!
|
||||
* \file cubature_filter.h
|
||||
* \brief Interface of a library with Bayesian noise statistic estimation
|
||||
* \file nonlinear_tracking.h
|
||||
* \brief Interface of a library for nonlinear tracking algorithms
|
||||
*
|
||||
* Cubature_Filter implements the functionality of the Cubature Kalman
|
||||
* Filter, which uses multidimensional cubature rules to estimate the
|
||||
* time evolution of a nonlinear system.
|
||||
* time evolution of a nonlinear system. Unscented_filter implements
|
||||
* an Unscented Kalman Filter which uses Unscented Transform rules to
|
||||
* perform a similar estimation.
|
||||
*
|
||||
* [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE
|
||||
* Transactions on Automatic Control, 54(6):1254–1269,2009.
|
||||
@@ -38,17 +40,18 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_CUBATURE_FILTER_H_
|
||||
#define GNSS_SDR_CUBATURE_FILTER_H_
|
||||
#ifndef GNSS_SDR_NONLINEAR_TRACKING_H_
|
||||
#define GNSS_SDR_NONLINEAR_TRACKING_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;
|
||||
class Model_Function
|
||||
{
|
||||
public:
|
||||
Model_Function(){};
|
||||
virtual arma::vec operator()(arma::vec input) = 0;
|
||||
virtual ~Model_Function() = default;
|
||||
};
|
||||
|
||||
@@ -81,4 +84,33 @@ private:
|
||||
arma::mat P_x_est;
|
||||
};
|
||||
|
||||
class Unscented_filter
|
||||
{
|
||||
public:
|
||||
// Constructors and destructors
|
||||
Unscented_filter();
|
||||
Unscented_filter(int nx);
|
||||
Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0);
|
||||
~Unscented_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
|
||||
@@ -800,6 +800,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA)
|
||||
${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
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc
|
||||
)
|
||||
if(${FILESYSTEM_FOUND})
|
||||
target_compile_definitions(trk_test PRIVATE -DHAS_STD_FILESYSTEM=1)
|
||||
|
||||
@@ -100,6 +100,7 @@ DECLARE_string(log_dir);
|
||||
|
||||
#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/unscented_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"
|
||||
|
||||
@@ -28,12 +28,13 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "cubature_filter.h"
|
||||
#include "nonlinear_tracking.h"
|
||||
#include <armadillo>
|
||||
#include <gtest/gtest.h>
|
||||
#include <random>
|
||||
|
||||
#define CUBATURE_TEST_N_TRIALS 1000
|
||||
#define CUBATURE_TEST_TOLERANCE 0.01
|
||||
|
||||
class Transition_Model : public Model_Function
|
||||
{
|
||||
@@ -127,8 +128,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
|
||||
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));
|
||||
EXPECT_TRUE(arma::approx_equal(ckf_x_pre, kf_x_pre, "absdiff", CUBATURE_TEST_TOLERANCE));
|
||||
EXPECT_TRUE(arma::approx_equal(ckf_P_x_pre, kf_P_x_pre, "absdiff", CUBATURE_TEST_TOLERANCE));
|
||||
|
||||
// Update Step
|
||||
kf_H = arma::randu<arma::mat>(ny, nx);
|
||||
@@ -151,8 +152,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest)
|
||||
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));
|
||||
EXPECT_TRUE(arma::approx_equal(ckf_x_post, kf_x_post, "absdiff", CUBATURE_TEST_TOLERANCE));
|
||||
EXPECT_TRUE(arma::approx_equal(ckf_P_x_post, kf_P_x_post, "absdiff", CUBATURE_TEST_TOLERANCE));
|
||||
|
||||
delete transition_function;
|
||||
delete measurement_function;
|
||||
|
||||
@@ -0,0 +1,161 @@
|
||||
/*!
|
||||
* \file unscented_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 "nonlinear_tracking.h"
|
||||
#include <armadillo>
|
||||
#include <gtest/gtest.h>
|
||||
#include <random>
|
||||
|
||||
#define UNSCENTED_TEST_N_TRIALS 10
|
||||
#define UNSCENTED_TEST_TOLERANCE 10
|
||||
|
||||
class Transition_Model_UKF : public Model_Function
|
||||
{
|
||||
public:
|
||||
Transition_Model_UKF(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_UKF : public Model_Function
|
||||
{
|
||||
public:
|
||||
Measurement_Model_UKF(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(UnscentedFilterComputationTest, UnscentedFilterTest)
|
||||
{
|
||||
Unscented_filter kf_unscented;
|
||||
|
||||
arma::vec kf_x;
|
||||
arma::mat kf_P_x;
|
||||
|
||||
arma::vec kf_x_pre;
|
||||
arma::mat kf_P_x_pre;
|
||||
|
||||
arma::vec ukf_x_pre;
|
||||
arma::mat ukf_P_x_pre;
|
||||
|
||||
arma::vec kf_x_post;
|
||||
arma::mat kf_P_x_post;
|
||||
|
||||
arma::vec ukf_x_post;
|
||||
arma::mat ukf_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 < UNSCENTED_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_unscented.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_UKF(kf_F);
|
||||
arma::mat ttx = (*transition_function)(kf_x_post);
|
||||
|
||||
kf_unscented.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q);
|
||||
|
||||
ukf_x_pre = kf_unscented.get_x_pred();
|
||||
ukf_P_x_pre = kf_unscented.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(ukf_x_pre, kf_x_pre, "absdiff", UNSCENTED_TEST_TOLERANCE));
|
||||
EXPECT_TRUE(arma::approx_equal(ukf_P_x_pre, kf_P_x_pre, "absdiff", UNSCENTED_TEST_TOLERANCE));
|
||||
|
||||
// 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_UKF(kf_H);
|
||||
kf_unscented.update_sequential(kf_y, kf_x_pre, kf_P_x_pre, measurement_function, kf_R);
|
||||
|
||||
ukf_x_post = kf_unscented.get_x_est();
|
||||
ukf_P_x_post = kf_unscented.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(ukf_x_post, kf_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE));
|
||||
EXPECT_TRUE(arma::approx_equal(ukf_P_x_post, kf_P_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE));
|
||||
|
||||
delete transition_function;
|
||||
delete measurement_function;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user