ukf_types.h

Go to the documentation of this file.

/* ukf_types.h
 * 
 * Copyright (C) 2011-2014 Jeremy Fix
 * 
 * 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, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef UKF_TYPES_H
#define UKF_TYPES_H

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include "ukf_math.h"

namespace ukf
{

    typedef enum
    {
        UKF_PROCESS_FIXED,
        UKF_PROCESS_RLS
    } ProcessNoise;      

    namespace parameter
    {
        class EvolutionNoise;
        typedef struct
        {
            double kpa;

            double alpha;

            double beta;

            double lambda;

            double gamma;

            //double evolution_noise_parameter;

            //double evolution_noise;

            //EvolutionNoise evolution_noise_type;
            EvolutionNoise * evolution_noise;

            double observation_noise;

            double prior_pi;

            int n;

            int nbSamples;

            int no;

        } ukf_param;

        //typedef double (*ukf_function_scalar) (gsl_vector * param, gsl_vector * input);

        typedef struct
        {
            gsl_vector * Kk;

            gsl_matrix * Kk_mat;

            gsl_matrix * Kk_mat_T;

            gsl_vector * Pwdk;

            gsl_matrix * Prrk;

            double Peek;

            double Pddk;

            gsl_vector * w;

            gsl_vector * wk;

            gsl_matrix * Pk;

            gsl_matrix * Sk;

            gsl_vector * cSk;

            gsl_vector * wm;

            gsl_vector * wc;

            gsl_vector * dk;

            double ino_dk;

            double d_mean;

            gsl_matrix * sigmaPoints;

            gsl_vector * temp_n;

            gsl_matrix * temp_n_n;

        } ukf_scalar_state;

        typedef struct
        {
            gsl_matrix * Kk;

            gsl_matrix * Kk_T;

            gsl_matrix * Pwdk;

            gsl_matrix * Pddk;

            gsl_matrix * Peek;

            gsl_matrix * Prrk;

            gsl_vector * w;

            gsl_vector * wk;

            gsl_matrix * Pk;

            gsl_matrix * Sk;

            gsl_vector * cSk;

            gsl_vector * wm;

            gsl_vector * wc;

            gsl_matrix * dk;

            gsl_vector * d_mean;

            gsl_vector * ino_dk;

            gsl_matrix * sigmaPoints;

            gsl_vector * vec_temp_n;

            gsl_vector * vec_temp_output;

            gsl_matrix * mat_temp_n_1;

            gsl_matrix * mat_temp_n_output;

            gsl_matrix * mat_temp_output_n;

            gsl_matrix * mat_temp_1_output;

            gsl_matrix * mat_temp_output_1;

            gsl_matrix * mat_temp_output_output;

            gsl_matrix * mat_temp_n_n;
        } ukf_state;

        class EvolutionNoise
        {
        protected:
            double _initial_value;
        public:
            EvolutionNoise(double initial_value) : _initial_value(initial_value) {};
            void init(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_set_identity(s.Prrk);
                gsl_matrix_scale(s.Prrk, _initial_value);
            }
            void init(ukf_param &p, ukf_scalar_state &s)
            {
                gsl_matrix_set_identity(s.Prrk);
                gsl_matrix_scale(s.Prrk, _initial_value);
            }

            virtual void updateEvolutionNoise(ukf_param &p, ukf_state &s) = 0;
            virtual void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s) = 0;
        };

        class EvolutionAnneal : public EvolutionNoise
        {
            double _decay, _lower_bound;
        public:
            EvolutionAnneal(double initial_value, double decay, double lower_bound) : EvolutionNoise(initial_value),
            _decay(decay),
            _lower_bound(lower_bound)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                            gsl_matrix_set(s.Prrk, i, i, ukf::math::max(_decay * gsl_matrix_get(s.Prrk,i,i),_lower_bound));
                        else
                            gsl_matrix_set(s.Prrk, i, j, 0.0);
                    }
                }
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                            gsl_matrix_set(s.Prrk, i, i, ukf::math::max(_decay * gsl_matrix_get(s.Prrk,i,i),_lower_bound));
                        else
                            gsl_matrix_set(s.Prrk, i, j, 0.0);
                    }
                }
            }
        };

        class EvolutionRLS : public EvolutionNoise
        {
            double _decay;
        public:
            EvolutionRLS(double initial_value, double decay) : EvolutionNoise(initial_value), _decay(decay)
            {
                if(ukf::math::cmp_equal(_decay, 0.0))
                    printf("Forgetting factor should not be null !!\n");
            };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_memcpy(s.Prrk, s.Pk);
                gsl_matrix_scale(s.Prrk, 1.0 / _decay - 1.0);
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                gsl_matrix_memcpy(s.Prrk, s.Pk);
                gsl_matrix_scale(s.Prrk, 1.0 / _decay - 1.0);
            }
        };

        class EvolutionRobbinsMonro : public EvolutionNoise
        {
            double _alpha;
        public:
            EvolutionRobbinsMonro(double initial_value, double alpha) : EvolutionNoise(initial_value),
            _alpha(alpha)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                // Compute Kk * ino_yk
              gsl_matrix_view mat_view = gsl_matrix_view_array(s.ino_dk->data, p.no, 1);
              gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.Kk, &mat_view.matrix, 0.0, s.mat_temp_n_1);
              // Compute : Prrk = (1 - alpha) Prrk + alpha . (Kk * ino_dk) * (Kk * ino_dk)^T
              gsl_blas_dgemm(CblasNoTrans, CblasTrans, _alpha, s.mat_temp_n_1, s.mat_temp_n_1, (1.0 - _alpha),s.Prrk);
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                // Compute  Prrk = (1 - alpha) Prrk + alpha . ino_dk^2 Kk Kk^T
              gsl_matrix_view mat_view = gsl_matrix_view_array(s.Kk->data,p.n,1);
                gsl_blas_dgemm(CblasNoTrans, CblasTrans, gsl_pow_2(s.ino_dk) * _alpha, &mat_view.matrix, &mat_view.matrix, 1.0 - _alpha, s.Prrk);
            }
        };


    } // parameter

    namespace state
    {

        class EvolutionNoise;
        typedef struct
        {
            double kpa;

            double alpha;

            double beta;

            double lambda;
            double lambda_aug;

            double gamma;
            double gamma_aug;

            int n;

            int nbSamples;

            int nbSamplesMeasure;

            int no;

            double prior_x;

            EvolutionNoise * evolution_noise;

            //double process_noise;

            double measurement_noise;

        } ukf_param;

        typedef struct
        {
            gsl_vector * params; // The parameters of the process equation or anything you need to give to the process function

            gsl_vector * xi; // State vector
            gsl_matrix * xi_prediction; // Prediction of the states for each sigma-point
            gsl_vector * xi_mean; // Mean state vector
            gsl_matrix * Pxxi; // State covariance matrix
            gsl_matrix * cholPxxi; // Cholesky decomposition of Pxxi
            gsl_matrix * Pvvi; // Process noise covariance
            gsl_matrix * cholPvvi; // Cholesky decomposition of the process noise

            gsl_matrix * yi_prediction; // Prediction of the measurements for each sigma-point
            gsl_vector * yi_mean; // Mean measurement vector
            gsl_vector * ino_yi; // Innovations
            gsl_matrix * Pyyi; // Measurement covariance matrix
            gsl_matrix * Pnni; // Measurement noise covariance

            gsl_matrix * Pxyi; // State-Measurement covariance matrix

            gsl_vector * sigmaPoint; // Vector holding one sigma point
            gsl_matrix * sigmaPoints;  // Matrix holding all the sigma points

            gsl_vector * sigmaPointMeasure; // one sigma point for the observation equation
            gsl_matrix * sigmaPointsMeasure; // Sigma points for the observation equation

            gsl_vector * wm_j; // Weights used to compute the mean of the sigma points images
            gsl_vector * wc_j; // Weights used to update the covariance matrices

            gsl_vector * wm_aug_j; // Augmented set of Weights used to compute the mean of the sigma points images for the observations
            gsl_vector * wc_aug_j; // Augmented set of Weights used to update the covariance matrices of the observations

            gsl_matrix * Ki; // Kalman gain
            gsl_matrix * Ki_T; // Its transpose

            gsl_vector * temp_n;
            gsl_matrix * temp_n_n;
            gsl_matrix * temp_n_1;
            gsl_matrix * temp_1_n;

            gsl_matrix * temp_n_no;

            gsl_matrix * temp_no_no;
            gsl_matrix * temp_no_1;
            gsl_matrix * temp_1_no;
        } ukf_state;

        class EvolutionNoise
        {
        protected:
            double _initial_value;
        public:
            EvolutionNoise(double initial_value) : _initial_value(initial_value) {};
            void init(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_set_identity(s.Pvvi);
                gsl_matrix_scale(s.Pvvi, _initial_value);
                gsl_matrix_set_identity(s.cholPvvi);
                gsl_matrix_scale(s.cholPvvi, sqrt(_initial_value));
            }

            virtual void updateEvolutionNoise(ukf_param &p, ukf_state &s) = 0;
        };

        class EvolutionAnneal : public EvolutionNoise
        {
            double _decay, _lower_bound;
        public:
            EvolutionAnneal(double initial_value, double decay, double lower_bound) : EvolutionNoise(initial_value),
            _decay(decay),
            _lower_bound(lower_bound)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                double value;
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                        {
                            value = ukf::math::max(_decay * gsl_matrix_get(s.Pvvi,i,i),_lower_bound);
                            gsl_matrix_set(s.Pvvi, i, i, value);
                            gsl_matrix_set(s.cholPvvi, i ,i, sqrt(value));
                        }
                        else
                        {
                            gsl_matrix_set(s.Pvvi, i, j, 0.0);
                            gsl_matrix_set(s.cholPvvi, i, j, 0.0);
                        }
                    }
                }
            }
        };

        class EvolutionRLS : public EvolutionNoise
        {
            double _decay;
        public:
            EvolutionRLS(double initial_value, double decay) : EvolutionNoise(initial_value), _decay(decay)
            {
                if(ukf::math::cmp_equal(_decay, 0.0))
                    printf("Forgetting factor should not be null !!\n");
            };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_memcpy(s.Pvvi, s.Pxxi);
                gsl_matrix_scale(s.Pvvi, 1.0 / _decay - 1.0);
                gsl_matrix_memcpy(s.cholPvvi, s.Pvvi);
                gsl_linalg_cholesky_decomp(s.cholPvvi);
                // Set all the elements of cholPvvi strictly above the diagonal to zero
                for(int j = 0 ; j < p.n ; j++)
                    for(int k = j+1 ; k < p.n ; k++)
                         gsl_matrix_set(s.cholPvvi,j,k,0.0);
            }
        };

        class EvolutionRobbinsMonro : public EvolutionNoise
        {
            double _alpha;
        public:
            EvolutionRobbinsMonro(double initial_value, double alpha) : EvolutionNoise(initial_value),
            _alpha(alpha)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                // Compute Kk * ino_yk
                gsl_blas_dgemv(CblasNoTrans, 1.0, s.Ki, s.ino_yi, 0.0, s.temp_n);
                // Compute : Prrk = (1 - alpha) Prrk + alpha . (Kk * ino_dk) * (Kk * ino_dk)^T
                gsl_matrix_view mat_view = gsl_matrix_view_array(s.temp_n->data,p.n,1);
                gsl_blas_dgemm(CblasNoTrans, CblasTrans, _alpha, &mat_view.matrix, &mat_view.matrix, (1.0 - _alpha),s.Pvvi);
            }
        };


    } // namespace for state estimation

    namespace srstate
    {


        typedef struct
        {
            double kpa;

            double alpha;

            double beta;

            double lambda;
            double lambda_aug;

            double gamma;
            double gamma_aug;

            int n;

            int nbSamples;

            int nbSamplesMeasure;

            int no;

            double prior_x;

            ProcessNoise process_noise_type;

            double process_noise;

            double measurement_noise;

        } ukf_param;

        typedef struct
        {
            gsl_vector * params; // The parameters of the process equation or anything you need to give to the process function

            gsl_vector * xi; // State vector
            gsl_matrix * xi_prediction; // Prediction of the states for each sigma-point
            gsl_vector * xi_mean; // Mean state vector
            gsl_matrix * Sxi; // Cholesky factor of variance covariance of the state
            gsl_matrix * Svi; // Cholesky factor of the process noise

            gsl_matrix * yi_prediction; // Prediction of the measurements for each sigma-point
            gsl_vector * yi_mean; // Mean measurement vector
            gsl_vector * ino_yi; // Innovations
            gsl_matrix * Syi; // Cholesky factor of the variance covariance of the output
            gsl_matrix * Sni; // Cholesky factor of the measurement noise

            gsl_matrix * Pxyi; // State-Measurement covariance matrix

            gsl_vector * sigmaPoint; // Vector holding one sigma point
            gsl_matrix * sigmaPoints;  // Matrix holding all the sigma points

            gsl_vector * sigmaPointMeasure; // one sigma point for the observation equation
            gsl_matrix * sigmaPointsMeasure; // Sigma points for the observation equation

            gsl_matrix * U;

            gsl_vector * wm_j; // Weights used to compute the mean of the sigma points images
            gsl_vector * wc_j; // Weights used to update the covariance matrices

            gsl_vector * wm_aug_j; // Augmented set of Weights used to compute the mean of the sigma points images for the observations
            gsl_vector * wc_aug_j; // Augmented set of Weights used to update the covariance matrices of the observations

            gsl_matrix * Ki; // Kalman gain
            gsl_matrix * Ki_T; // Its transpose

            gsl_vector * temp_n;
            gsl_vector * temp_no;

            gsl_matrix * temp_n_n;
            gsl_matrix * temp_n_1;
            gsl_matrix * temp_1_n;

            gsl_matrix * temp_n_no;

            gsl_matrix * temp_no_no;
            gsl_matrix * temp_no_1;
            gsl_matrix * temp_1_no;

            gsl_matrix * temp_3n_n;
            gsl_matrix * temp_2nno_no;
        } ukf_state;        


    } // namespace for state estimation, square root implementation

}


#endif // UKF_TYPES_H