144 lines
4.2 KiB
C++
144 lines
4.2 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),
|
|
d_last_x(0.0)
|
|
{
|
|
}
|
|
|
|
/**
|
|
* This method calculates the coefficients of the polynomial that will
|
|
* be used by the predict_freqs() method to produce simulated frequency
|
|
* differences
|
|
* @param drifts the queue containing the frequency differences and the
|
|
* corresponding timings that these frequencies diffrences occured. Time is
|
|
* measured in samples since the start of the flowgraph execution.
|
|
*/
|
|
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);
|
|
|
|
/*
|
|
* Lock the mutex to make sure that no one uses at the same time the
|
|
* coefficients
|
|
*/
|
|
boost::mutex::scoped_lock lock(d_mutex);
|
|
d_coeff = std::vector<double> (txy_matrix.data().begin(),
|
|
txy_matrix.data().end());
|
|
d_last_x = drifts[s - 1].get_x();
|
|
}
|
|
|
|
/**
|
|
* Creates a number of frequency differences predictions using polynomial
|
|
* curve fitting.
|
|
* @param freqs buffer that will hold the predicted frequency differences.
|
|
* It is responsibility of the caller to provide enough memory for at most
|
|
* \p ncorrections double numbers.
|
|
* @param ncorrections the number predicted frequencies that the method
|
|
* will produce.
|
|
* @param samples_per_correction the number of samples elapsed between each
|
|
* correction.
|
|
*/
|
|
void
|
|
doppler_fit::predict_freqs(double *freqs, size_t ncorrections,
|
|
size_t samples_per_correction)
|
|
{
|
|
size_t i;
|
|
size_t j;
|
|
double predicted_freq_diff;
|
|
double x;
|
|
double xT;
|
|
boost::mutex::scoped_lock lock(d_mutex);
|
|
for (i = 0; i < ncorrections; i++) {
|
|
predicted_freq_diff = 0.0;
|
|
xT = 1.0;
|
|
x = d_last_x + i * samples_per_correction;
|
|
for (j = 0; j < d_degree + 1; j++) {
|
|
predicted_freq_diff += d_coeff[j] * xT;
|
|
xT *= x;
|
|
}
|
|
freqs[i] = predicted_freq_diff;
|
|
}
|
|
|
|
/*
|
|
* The predict method can be called multiple times without update the
|
|
* fitness of the polynomial. For this reason we alter the last x
|
|
*/
|
|
d_last_x = d_last_x + (ncorrections + 1) * samples_per_correction;
|
|
}
|
|
|
|
} /* namespace satnogs */
|
|
} /* namespace gr */
|
|
|