KallmanFilter.cpp

/**
 * DESCRIPTION OF THE FILE
 *
 * @author Michal Kravčenko
 * @date 25.2.19 -
 */

#include <armadillo>
#include "KallmanFilter.h"
namespace lib4neuro {

    std::vector<double> *KallmanFilter::get_parameters() {
        return this->optimal_parameters;
    }

    void KallmanFilter::optimize(ErrorFunction &ef, std::ofstream *ofs) {
/* Copy data set max and min values, if it's normalized */
        if(ef.get_dataset()->is_normalized()) {
            ef.get_network_instance()->set_normalization_strategy_instance(
                    ef.get_dataset()->get_normalization_strategy());
        }

        double current_err = ef.eval();

        COUT_INFO("Finding a solution via a Levenberg-Marquardt method... Starting error: " << current_err << std::endl);
        if(ofs && ofs->is_open()) {
            *ofs << "Finding a solution via a Levenberg-Marquardt method... Starting error: " << current_err << std::endl;
        }

        DataSet *data = ef.get_dataset();
        size_t dim_train_set = data->get_n_elements();
        size_t dim_out = ef.get_network_instance()->get_n_outputs() * dim_train_set;
        size_t n_parameters = ef.get_dimension();

        this->optimal_parameters->resize(n_parameters);

        arma::Mat<double> H, P, K_partial_i, K, P_new, I;

        //-------------------//
        // Solver iterations //
        //-------------------//
        arma::Col<double> solution(*ef.get_parameters()), solution_mem(n_parameters), error(dim_out);

        std::random_device seeder;
        std::mt19937 gen(seeder());
        std::uniform_real_distribution<double> new_entry(-1, 1);

        P.reshape(n, n);
        for( size_t r = 0; r < n; ++r ){
            for( size_t c = 0; c < n; ++c ){
                P.at( r, c ) = new_entry(gen);
            }
        }

        I = arma::eye(n_parameters, n_parameters);
        H.reshape(dim_out, n_parameters);

        /* Control Parameters */
        double jacobian_norm = 0.0, error_norm = 0.0, error;
        int iter_idx = this->max_iters - 1, it = 0;

        std::vector<std::vector<double>> jacobian;
        std::vector<double> partial_error;

        while( error_norm > this->tolerance && iter_idx != 0 ){
            it++;
            /* Jacobian and error vector recalculation */

            H.fill(0.0);
            size_t row_idx = 0;
            for ( auto item: *data.get_data() ){
                f.get_jacobian( jacobian, item, partial_error );

                for(size_t ri = 0; ri < jacobian.size(); ++ri){
                    for(size_t ci = 0; ci < dim_params; ++ci){
                        H.at(row_idx, ci) = jacobian[ri][ci];
                        error.at(row_idx) = partial_error[ri];
                    }
                    row_idx++;
                }
            }
            /* End of Jacobian and error vector recalculation */

            K_partial_i = H*P*H.t();
            K = P * H * (K_partial_i.i());

            solution = solution + K * error;

            P_new = (I - K * H) * P;
            P = P_new;

            error_norm = 0.0;
            for( auto e: error ){
                error_norm += e * e;
            }
            error_norm = std::sqrt(error_norm);
            COUT_DEBUG("Iteration: " << it << " Current error: " << error_norm << std::endl);
        }

        COUT_DEBUG("Iteration: " << it << " Current error: " << error_norm << std::endl);

        /* Store the optimized parameters */
        for( size_t i = 0; i < solution.size(); ++i ){
            this->optimal_parameters->at( i ) = solution.at( i );
        }

        ef.get_network_instance()->copy_parameter_space(this->optimal_parameters);
    }

    KallmanFilter::KallmanFilter(double tol, int max_it) {
        this->tolerance = tol;
        this->max_iters = max_it;
        this->optimal_parameters = new std::vector<double>();
    }

    KallmanFilter::~KallmanFilter() {
        if( this->optimal_parameters ){
            delete this->optimal_parameters;
        }
    }

}