95 lines
2.6 KiB
C++
95 lines
2.6 KiB
C++
|
/* -*- c++ -*- */
|
||
|
|
/*
|
||
|
|
* gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
|
||
|
|
*
|
||
|
|
* Copyright (C) 2016, Libre Space Foundation <http://librespacefoundation.org/>
|
||
|
|
*
|
||
|
|
* This program 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.
|
||
|
|
*
|
||
|
|
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifdef HAVE_CONFIG_H
|
||
|
|
#include "config.h"
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include <gnuradio/io_signature.h>
|
||
|
|
#include <satnogs/doppler_fit.h>
|
||
|
|
#include <boost/numeric/ublas/matrix.hpp>
|
||
|
|
#include <boost/numeric/ublas/lu.hpp>
|
||
|
|
|
||
|
|
namespace gr
|
||
|
|
{
|
||
|
|
namespace satnogs
|
||
|
|
{
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Creates a polynomial fitting routine.
|
||
|
|
* @param degree the degree of the polynomial
|
||
|
|
*/
|
||
|
|
doppler_fit::doppler_fit (size_t degree) :
|
||
|
|
d_degree(degree)
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
doppler_fit::~doppler_fit ()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
void
|
||
|
|
doppler_fit::fit (std::deque<freq_drift> drifts)
|
||
|
|
{
|
||
|
|
size_t i;
|
||
|
|
size_t j;
|
||
|
|
size_t s;
|
||
|
|
size_t singular;
|
||
|
|
double val;
|
||
|
|
s = drifts.size();
|
||
|
|
|
||
|
|
boost::numeric::ublas::matrix<double> x_matrix(s, d_degree+1);
|
||
|
|
boost::numeric::ublas::matrix<double> y_matrix(s, 1);
|
||
|
|
|
||
|
|
for(i = 0; i < s; i++){
|
||
|
|
y_matrix(i, 0) = drifts[i].get_freq_drift();
|
||
|
|
}
|
||
|
|
|
||
|
|
for(i = 0; i < s; i++){
|
||
|
|
val = 1.0;
|
||
|
|
for(j = 0; j < d_degree + 1; j++) {
|
||
|
|
x_matrix(i, j) = val;
|
||
|
|
val *= drifts[i].get_x();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Transpose the matrix with the x values */
|
||
|
|
boost::numeric::ublas::matrix<double> tx_matrix (
|
||
|
|
boost::numeric::ublas::trans (x_matrix));
|
||
|
|
|
||
|
|
boost::numeric::ublas::matrix<double> txx_matrix (
|
||
|
|
boost::numeric::ublas::prec_prod(tx_matrix, x_matrix));
|
||
|
|
|
||
|
|
boost::numeric::ublas::matrix<double> txy_matrix (
|
||
|
|
boost::numeric::ublas::prec_prod(tx_matrix, y_matrix));
|
||
|
|
|
||
|
|
boost::numeric::ublas::permutation_matrix<int> perm(txx_matrix.size1());
|
||
|
|
singular = boost::numeric::ublas::lu_factorize(txx_matrix, perm);
|
||
|
|
BOOST_ASSERT( singular == 0 );
|
||
|
|
|
||
|
|
boost::numeric::ublas::lu_substitute(txx_matrix, perm, txy_matrix);
|
||
|
|
d_coeff = std::vector<double> (txy_matrix.data().begin(),
|
||
|
|
txy_matrix.data().end());
|
||
|
|
}
|
||
|
|
|
||
|
|
} /* namespace satnogs */
|
||
|
|
} /* namespace gr */
|
||
|
|
|