mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-10-31 15:23:04 +00:00
Add tracking lib for cubature kalman filter
This commit is contained in:
Gerald LaMountain
@@ -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
|
||||
|
||||
182
src/algorithms/tracking/libs/cubature_filter.cc
Normal file
182
src/algorithms/tracking/libs/cubature_filter.cc
Normal file
@@ -0,0 +1,182 @@
|
||||
/*!
|
||||
* \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] TODO: Refs
|
||||
*
|
||||
* \authors <ul>
|
||||
* <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
|
||||
* <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
|
||||
* </ul>
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (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::init(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, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance)
|
||||
{
|
||||
// Compute number of cubature points
|
||||
int nx = x_post.n_elem;
|
||||
int np = 2 * 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);
|
||||
|
||||
// Propagate and evaluate cubature points
|
||||
arma::vec Xi_post;
|
||||
arma::vec Xi_pred;
|
||||
for (int32_t i = 0; i < np; i++)
|
||||
{
|
||||
Xi_post = Sm_post*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + x_post;
|
||||
Xi_pred = (*transition_fcn)(Xi_post);
|
||||
|
||||
x_pred = x_post + Xi_pred;
|
||||
P_x_pred = P_x_post + 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, arma::vec (*measurement_fcn)(const arma::mat&), 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;
|
||||
|
||||
// Evaluate predicted measurement and covariances
|
||||
arma::mat z_pred = arma::zeros(nx,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);
|
||||
|
||||
// Propagate and evaluate cubature points
|
||||
arma::vec Xi_pred;
|
||||
arma::vec Zi_pred;
|
||||
for (int32_t i = 0; i < np; i++)
|
||||
{
|
||||
Xi_pred = Sm_pred*std::sqrt(((float) np) / 2.0)*arma::ones(nx,1) + 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;
|
||||
}
|
||||
74
src/algorithms/tracking/libs/cubature_filter.h
Normal file
74
src/algorithms/tracking/libs/cubature_filter.h
Normal file
@@ -0,0 +1,74 @@
|
||||
/*!
|
||||
* \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] TODO: Refs
|
||||
*
|
||||
* \authors <ul>
|
||||
* <li> Gerald LaMountain, 2019. gerald(at)ece.neu.edu
|
||||
* <li> Jordi Vila-Valls 2019. jvila(at)cttc.es
|
||||
* </ul>
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (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>
|
||||
|
||||
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 init(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, arma::vec (*transition_fcn)(const arma::mat&), const arma::mat& noise_covariance);
|
||||
void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, arma::vec (*measurement_fcn)(const arma::mat&), 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
|
||||
Reference in New Issue
Block a user