root/applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

/*!
  \file
  \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertaint16y

  Using IT++ for numerical operations
  -----------------------------------
*/

#ifndef EKFfix_H
#define EKFfix_H


#include <estim/kalman.h>
#include "fixed.h"
#include "matrix.h"
#include "matrix_vs.h"
#include "reference_Q15.h"
#include "parametry_motoru.h"
#include "mpf_double.h"
#include "fast_exp.h"
#include "ekf_mm.h"
#include "qmath.h"

using namespace bdm;

double minQ(double Q); 

void mat_to_int16(const imat &M, int16 *I);
void vec_to_int16(const ivec &v, int16 *I);
void int16_to_mat(int16 *I, imat &M, int rows, int cols);
void int16_to_vec(int16 *I, ivec &v, int len);
void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref);


//! EKF for testing q44
class EKFtest: public EKF_UD{
        void bayes ( const vec &yt, const vec &cond ) {
                EKF_UD::bayes(yt,cond);
                vec D =  prior()._R()._D();
                
                if (D(3)>10) D(3) = 10;
                
                prior()._R().__D()=D;
        }
};
UIREGISTER(EKFtest);


class EKFscale: public EKFCh{
        LOG_LEVEL(EKFscale, logCh, logA, logC);
public:
        mat Tx;
        mat Ty;
        
        void from_setting ( const Setting &set ){
                EKFCh::from_setting(set);
                vec v;
                UI::get(v, set, "xmax", UI::compulsory);
                Tx = diag(1./v);
                UI::get(v, set, "ymax", UI::compulsory);
                Ty = diag(1./v);
                
                UI::get(log_level, set, "log_level", UI::optional);
        };
        
        void log_register(logger &L, const string &prefix){
                BM::log_register ( L, prefix );
                
                L.add_vector ( log_level, logCh, RV ("Ch", dimension()*dimension() ), prefix );
                L.add_vector ( log_level, logA, RV ("A", dimension()*dimension() ), prefix );
                L.add_vector ( log_level, logC, RV ("C", dimensiony()*dimension() ), prefix );
        };
        
        void log_write() const{
                BM::log_write();
            if ( log_level[logCh] ) {
                        mat Chsc=Tx*(est._R()._Ch());
                        vec v(Chsc._data(), dimension()*dimension());
                        if (v(0)<0)
                                v= -v;
                        log_level.store( logCh, round(v*32767));
                }
                if (log_level[logA]){
                        mat Asc = Tx*A*inv(Tx);
                        vec v(Asc._data(), dimension()*dimension());
                        log_level.store( logA, round(v*32767));
                }
                if (log_level[logC]){
                        mat Csc = Ty*C*inv(Tx);
                        vec v(Csc._data(), dimensiony()*dimension());
                        log_level.store( logC, round(v*32767));
                }
        }
        
};
UIREGISTER(EKFscale);

/*!
\brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic

An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
*/
class EKFfixedUD : public BM {
        public:
                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP);
                
                void init_ekf(double Tv);
                void ekf(double ux, double uy, double isxd, double isyd);
                                
                /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
                int16 Q[16]; /* matrix [4,4] */
                int16 R[4]; /* matrix [2,2] */
                
                int16 x_est[4]; /* estimate and prediction */
                
                int16 PSI[16]; /* matrix [4,4] */
                int16 PSIU[16]; /* matrix PIS*U, [4,4] */
                
                int16 Uf[16]; // upper triangular of covariance (inplace)
                int16 Df[4];  // diagonal covariance
                int16 Dfold[4]; // temp of D
                int16 G[16];  // temp for bierman
                
                int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
                
                enorm<fsqmat> E;
                mat Ry;
                
        public:
                //! Default constructor
                EKFfixedUD ():BM(),E(),Ry(2,2){
                        int16 i;
                        for(i=0;i<16;i++){Q[i]=0;}
                        for(i=0;i<4;i++){R[i]=0;}
                        
                        for(i=0;i<4;i++){x_est[i]=0;}
                        for(i=0;i<16;i++){Uf[i]=0;}
                        for(i=0;i<4;i++){Df[i]=0;}
                        for(i=0;i<16;i++){G[i]=0;}
                        for(i=0;i<4;i++){Dfold[i]=0;}
                        
                        for(i=0;i<16;i++){PSI[i]=0;}
                        
                        set_dim(4);
                        dimy = 2;
                        dimc = 2;
                        E._mu()=zeros(4);
                        E._R()=zeros(4,4);
                        init_ekf(0.000125);
                };
                //! Here dt = [yt;ut] of appropriate dimensions
                void bayes ( const vec &yt, const vec &ut );
                //!dummy!
                const epdf& posterior() const {return E;};
                void log_register(logger &L, const string &prefix){
                        BM::log_register ( L, prefix );
                        
                                L.add_vector ( log_level, logG, RV("G",16), prefix );
                                L.add_vector ( log_level, logU, RV ("U", 16 ), prefix );
                                L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
                                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
                                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
                                
                };
                //void from_setting();
};

UIREGISTER(EKFfixedUD);

/*!
 * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedUD2 : public BM {
public:
        LOG_LEVEL(EKFfixedUD2,logU, logG, logD, logA, logC, logP);
        
        void init_ekf2(double Tv);
        void ekf2(double ux, double uy, double isxd, double isyd);
        
        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
        int16 Q[4]; /* matrix [4,4] */
        int16 R[4]; /* matrix [2,2] */
        
        int16 x_est[2]; /* estimate and prediction */
        int16 y_est[2]; /* estimate and prediction */
        int16 y_old[2]; /* estimate and prediction */
        
        int16 PSI[4]; /* matrix [4,4] */
        int16 PSIU[4]; /* matrix PIS*U, [4,4] */
        int16 C[4]; /* matrix [4,4] */
        
        int16 Uf[4]; // upper triangular of covariance (inplace)
        int16 Df[2];  // diagonal covariance
        int16 Dfold[2]; // temp of D
        int16 G[4];  // temp for bierman
        
        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
        
        enorm<fsqmat> E;
        mat Ry;
        
public:
        //! Default constructor
        EKFfixedUD2 ():BM(),E(),Ry(2,2){
                int16 i;
                for(i=0;i<4;i++){Q[i]=0;}
                for(i=0;i<4;i++){R[i]=0;}
                
                for(i=0;i<2;i++){x_est[i]=0;}
                for(i=0;i<2;i++){y_est[i]=0;}
                for(i=0;i<2;i++){y_old[i]=0;}
                for(i=0;i<4;i++){Uf[i]=0;}
                for(i=0;i<2;i++){Df[i]=0;}
                for(i=0;i<4;i++){G[i]=0;}
                for(i=0;i<2;i++){Dfold[i]=0;}
                
                for(i=0;i<4;i++){PSI[i]=0;}
                for(i=0;i<4;i++){C[i]=0;}
                
                set_dim(2);
                dimc = 2;
                dimy = 2;
                E._mu()=zeros(2);
                E._R()=zeros(2,2);
                init_ekf2(0.000125);
        };
        //! Here dt = [yt;ut] of appropriate dimensions
        void bayes ( const vec &yt, const vec &ut );
        //!dummy!
        const epdf& posterior() const {return E;};
        void log_register(logger &L, const string &prefix){
                BM::log_register ( L, prefix );
                
                L.add_vector ( log_level, logG, RV("G2",4), prefix );
                L.add_vector ( log_level, logU, RV ("U2", 4 ), prefix );
                L.add_vector ( log_level, logD, RV ("D2", 2 ), prefix );
                L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix );
                L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
                L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
                
        };
        //void from_setting();
};

UIREGISTER(EKFfixedUD2);

/*!
 * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedUD3 : public BM {
public:
        LOG_LEVEL(EKFfixedUD3,logU, logG, logD, logA, logC, logP);
        
        void init_ekf3(double Tv);
        void ekf3(double ux, double uy, double isxd, double isyd);
        
        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
        int16 Q[9]; /* matrix [4,4] */
        int16 R[4]; /* matrix [2,2] */
        
        int16 x_est[3]; /* estimate and prediction */
        int16 y_est[2]; /* estimate and prediction */
        int16 y_old[2]; /* estimate and prediction */
        
        int16 PSI[9]; /* matrix [4,4] */
        int16 PSIU[9]; /* matrix PIS*U, [4,4] */
        int16 C[6]; /* matrix [4,4] */
        
        int16 Uf[9]; // upper triangular of covariance (inplace)
        int16 Df[3];  // diagonal covariance
        int16 Dfold[3]; // temp of D
        int16 G[9];  // temp for bierman
        
        int16 cA, cB, cC, cG, cF, cH;  // cD, cE, cF, cI ... nepouzivane
        
        enorm<fsqmat> E;
        mat Ry;
        
public:
        //! Default constructor
        EKFfixedUD3 ():BM(),E(),Ry(2,2){
                int16 i;
                for(i=0;i<9;i++){Q[i]=0;}
                for(i=0;i<4;i++){R[i]=0;}
                
                for(i=0;i<3;i++){x_est[i]=0;}
                for(i=0;i<2;i++){y_est[i]=0;}
                for(i=0;i<2;i++){y_old[i]=0;}
                for(i=0;i<9;i++){Uf[i]=0;}
                for(i=0;i<3;i++){Df[i]=0;}
                for(i=0;i<4;i++){G[i]=0;}
                for(i=0;i<3;i++){Dfold[i]=0;}
                
                for(i=0;i<9;i++){PSI[i]=0;}
                for(i=0;i<6;i++){C[i]=0;}
                
                set_dim(3);
                dimc = 2;
                dimy = 2;
                E._mu()=zeros(3);
                E._R()=zeros(3,3);
                init_ekf3(0.000125);
        };
        //! Here dt = [yt;ut] of appropriate dimensions
        void bayes ( const vec &yt, const vec &ut );
        //!dummy!
        const epdf& posterior() const {return E;};
        void log_register(logger &L, const string &prefix){
                BM::log_register ( L, prefix );         
        };
        //void from_setting();
};

UIREGISTER(EKFfixedUD3);

/*!
 * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedCh : public BM {
public:
        LOG_LEVEL(EKFfixedCh,logCh, logA, logP);
        
        void init_ekf(double Tv);
        void ekf(double ux, double uy, double isxd, double isyd);
        
        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
        int16 Q[16]; /* matrix [4,4] */
        int16 R[4]; /* matrix [2,2] */
        
        int16 x_est[4]; /* estimate and prediction */
        
        int16 PSI[16]; /* matrix [4,4] */
        int16 PSICh[16]; /* matrix PIS*U, [4,4] */
        
        int16 Chf[16]; // upper triangular of covariance (inplace)
        
        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
        
        enorm<chmat> E;
        mat Ry;
        
public:
        //! Default constructor
        EKFfixedCh ():BM(),E(),Ry(2,2){
                int16 i;
                for(i=0;i<16;i++){Q[i]=0;}
                for(i=0;i<4;i++){R[i]=0;}
                
                for(i=0;i<4;i++){x_est[i]=0;}
                for(i=0;i<16;i++){Chf[i]=0;}
                
                for(i=0;i<16;i++){PSI[i]=0;}
                
                set_dim(4);
                dimc = 2;
                dimy =2;
                E._mu()=zeros(4);
                E._R()=zeros(4,4);
                init_ekf(0.000125);
        };
        //! Here dt = [yt;ut] of appropriate dimensions
        void bayes ( const vec &yt, const vec &ut );
        //!dummy!
        const epdf& posterior() const {return E;};
        void log_register(logger &L, const string &prefix){
                BM::log_register ( L, prefix );
                
                L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix );
                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );                
        };
        //void from_setting();
};

UIREGISTER(EKFfixedCh);

/*!
 * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedCh2 : public BM {
public:
        LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP, logDet, logRem);
        
        void init_ekf2(double Tv);
        void ekf2(double ux, double uy, double isxd, double isyd);
        
        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
        int16 Q[4]; /* matrix [4,4] */
        int16 R[4]; /* matrix [2,2] */
        
        int16 x_est[2]; /* estimate and prediction */
        int16 y_est[2]; /* estimate and prediction */
        int16 y_old[2]; /* estimate and prediction */
        
        int16 PSI[4]; /* matrix [4,4] */
        int16 PSICh[4]; /* matrix PIS*U, [4,4] */
        int16 C[4]; /* matrix [4,4] */
        
        int16 Chf[4]; // upper triangular of covariance (inplace)
        
        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
        
        enorm<fsqmat> E;
        mat Ry;
        
public:
        //! Default constructor
        EKFfixedCh2 ():BM(),E(),Ry(2,2){
                int16 i;
                for(i=0;i<4;i++){Q[i]=0;}
                for(i=0;i<4;i++){R[i]=0;}
                
                for(i=0;i<2;i++){x_est[i]=0;}
                for(i=0;i<2;i++){y_est[i]=0;}
                for(i=0;i<2;i++){y_old[i]=0;}
                for(i=0;i<4;i++){Chf[i]=0;}
                
                for(i=0;i<4;i++){PSI[i]=0;}
                for(i=0;i<4;i++){C[i]=0;}
                
                set_dim(2);
                dimc = 2;
                dimy = 2;
                E._mu()=zeros(2);
                E._R()=zeros(2,2);
                init_ekf2(0.000125);
        };
        //! Here dt = [yt;ut] of appropriate dimensions
        void bayes ( const vec &yt, const vec &ut );
        //!dummy!
        const epdf& posterior() const {return E;};
        void log_register(logger &L, const string &prefix){
                BM::log_register ( L, prefix );
                
                L.add_vector ( log_level, logCh, RV ("Ch2", 4 ), prefix );
                L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix );
                L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
                L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
                L.add_vector ( log_level, logDet, RV ("Det", 1 ), prefix );
                L.add_vector ( log_level, logRem, RV ("Rem", 1 ), prefix );
                
        };
        void from_setting ( const Setting &set ){
                BM::from_setting(set);
                vec dQ,dR;
                UI::get ( dQ, set, "dQ", UI::optional );
                UI::get ( dQ, set, "dR", UI::optional );
                if (dQ.length()==2){
                        Q[0]=prevod(dQ[0],15);      // 1e-3
                        Q[3]=prevod(dQ[1],15);      // 1e-3
                }
                if (dR.length()==2){
                        R[0]=prevod(dR[0],15);      // 1e-3
                        R[3]=prevod(dR[1],15);      // 1e-3
                }
        }
};

UIREGISTER(EKFfixedCh2);


//! EKF for comparison of EKF_UD with its fixed-point16 implementation
class EKF_UDfix : public BM {
        protected:
                //! logger
                LOG_LEVEL(EKF_UDfix,logU, logG);
                //! Internal Model f(x,u)
                shared_ptr<diffbifn> pfxu;
                
                //! Observation Model h(x,u)
                shared_ptr<diffbifn> phxu;
                
                //! U part
                mat U;
                //! D part
                vec D;
                                
                mat A;
                mat C;
                mat Q;
                vec R;
                
                enorm<ldmat> est;
                
                
        public:
                
                //! copy constructor duplicated 
                EKF_UDfix* _copy() const {
                        return new EKF_UDfix(*this);
                }
                
                const enorm<ldmat>& posterior()const{return est;};
                
                enorm<ldmat>& prior() {
                        return const_cast<enorm<ldmat>&>(posterior());
                }
                
                EKF_UDfix(){}
                
                
                EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){}
                
                //! Set nonlinear functions for mean values and covariance matrices.
                void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 );
                
                //! Here dt = [yt;ut] of appropriate dimensions
                void bayes ( const vec &yt, const vec &cond = empty_vec );
                
                void log_register ( bdm::logger& L, const string& prefix ){
                        BM::log_register ( L, prefix );
                        
                        if ( log_level[logU] )
                                L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix );
                        if ( log_level[logG] )
                                L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix );
                        
                }
                /*! Create object from the following structure
                
                \code
                class = 'EKF_UD';
                OM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
                IM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
                dQ = [...];                             % vector containing diagonal of Q
                dR = [...];                             % vector containing diagonal of R
                --- optional fields ---
                mu0 = [...];                            % vector of statistics mu0
                dP0 = [...];                            % vector containing diagonal of P0
                -- or --
                P0 = [...];                             % full matrix P0
                --- inherited fields ---
                bdm::BM::from_setting
                \endcode
                If the optional fields are not given, they will be filled as follows:
                \code
                mu0 = [0,0,0,....];                     % empty statistics
                P0 = eye( dim );              
                \endcode
                */
                void from_setting ( const Setting &set );
                
                void validate() {};
                // TODO dodelat void to_setting( Setting &set ) const;
                
};
UIREGISTER(EKF_UDfix);


class MPF_pmsm_red:public BM{
                double qom, qth, r;


public:
                MPF_pmsm_red(){
                        dimy=2;
                        dimc=2;
                        qom=1e-1;
                        qth=1e-6;
                        r=1e-1;
                };
        void bayes ( const vec &val, const vec &cond ) {
/*              const double &isa = val(0);
                const double &isb = val(1);
                const double &usa = cond(0);
                const double &usb = cond(1);*/
                mpf_bayes((floatx)val(0),(floatx)val(1),(floatx)cond(0), (floatx)cond(1));//isa,isb,usa,usb);
        }
        
        class mp:public epdf{
                LOG_LEVEL(mp,logth,logom);
        public:
                mp():epdf(){set_dim(3);         log_level[logth]=true;log_level[logom]=true;
}
                vec sample() const {return zeros(3);}
                double evallog(const vec &v) const {return 0.0;}
                vec mean() const {vec tmp(3); 
                        floatx es,ec,eo;
                        mpf_mean(&es, &ec, &eo); 
                        tmp(0)=es;tmp(1)=ec;tmp(2)=eo;
                        return tmp;
                }
                vec variance() const {return zeros(3);}
                void log_register ( bdm::logger& L, const string& prefix ) {
                        epdf::log_register ( L, prefix );
                        if ( log_level[logth] ) {
                                int th_dim = N; // dimension - dimension of cov
                                L.add_vector( log_level, logth, RV ( th_dim ), prefix );
                        }
                        if ( log_level[logom] ) {
                                int th_dim = N; // dimension - dimension of cov
                                L.add_vector( log_level, logom, RV ( th_dim ), prefix );
                        }
                }
                void log_write() const {
                        epdf::log_write();
                        if ( log_level[logth] ) {
                                floatx Th[N];
                                mpf_th(Th);
                                vec th(N); 
                                for(int i=0;i<N;i++){th(i)=Th[i];}
                                log_level.store( logth, th );
                        }
                        if ( log_level[logom] ) {
                                floatx Om[N];
                                mpf_om(Om);
                                vec om(N); 
                                for(int i=0;i<N;i++){om(i)=Om[i];}
                                log_level.store( logom, om );
                        }
                }
        };
        
        mp mypdf;
        const mp& posterior() const {return mypdf;}
        
        void from_setting(const Setting &set){
            BM::from_setting(set);
                UI::get ( log_level, set, "log_level", UI::optional );

                UI::get(qom,set,"qom",UI::optional);
                UI::get(qth,set,"qth",UI::optional);
                UI::get(r,set,"r",UI::optional);
        }
        void validate(){
                mpf_init((floatx)qom,(floatx)qth,(floatx)r);
                
        }
};
UIREGISTER(MPF_pmsm_red);

//! EKF with covariance R estimated by the VB method
class EKFvbR: public EKFfull{   
    LOG_LEVEL(EKFvbR,logR,logQ,logP);

        //! Statistics of the R estimator
        mat PsiR;
        //! Statistics of the Q estimator
        mat PsiQ;
        //! forgetting factor
        double phi;
        //! number of VB iterations
        int niter;
        //! degrees of freedom
        double nu;
        //! stabilizing element
        mat PsiR0;
        //! stabilizing element
        mat PsiQ0;
        
        void from_setting(const Setting &set){
            EKFfull::from_setting(set);
                if (!UI::get(phi,set,"phi",UI::optional)){
                        phi = 0.99;
                }
                if (!UI::get(niter,set,"niter",UI::optional)){
                        niter = 3;
                }
                PsiQ = Q;
                PsiQ0 = Q;
                PsiR = R;
                PsiR0 = R;
                nu = 3;
    }   
        void log_register ( logger &L, const string &prefix ) {
                EKFfull::log_register(L,prefix);
                L.add_vector(log_level, logR, RV("{R }",vec_1<int>(4)), prefix);
                L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(4)), prefix);
                L.add_vector(log_level, logP, RV("{P }",vec_1<int>(4)), prefix);
        }
        void bayes ( const vec &val, const vec &cond ) {
                vec diffx, diffy;
                mat Psi_vbQ;
                mat Psi_vbR;
                nu = phi*nu + (1-phi)*2 + 1.0;
                
                //save initial values of posterior
                vec mu0=est._mu();
                fsqmat P0=est._R();
                vec xpred = pfxu->eval(mu0,cond);
                
                for (int i=0; i<niter; i++){
                        est._mu() = mu0;
                        est._R() = P0;
                        
                        EKFfull::bayes(val,cond);

//                      diffy = val - fy._mu();
//                      Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/;
//                      R = Psi_vbR/(nu-2);
        
                        diffx = est._mu() - xpred;
                        Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx);/*mat(est._R());  A*mat(P0)*A.T();*/
                        //
                        Psi_vbQ(0,1) = 0.0;
                        Psi_vbQ(1,0) = 0.0;
                        Q = Psi_vbQ/(nu-2);
                }
                PsiQ = Psi_vbQ;
                PsiR = Psi_vbR;
//              cout <<"==" << endl << Psi << endl << diff << endl << P0 << endl << ":" << Q;
                log_level.store(logQ, vec(Q._M()._data(),4));
                log_level.store(logR, vec(R._M()._data(),4));
                {
                        mat Ch(2,2);
                        Ch=chol(est._R()._M());
                        log_level.store(logP, vec(Ch._data(),4));
                }
        }
};
UIREGISTER(EKFvbR);


class ekfChfix: public BM{
        ekf_data E;
public:
        ekfChfix(){
                init_ekfCh2(&E,0.000125);set_dim(2);    dimc = 2;
                dimy = 2;
        } 
        void bayes ( const vec &val, const vec &cond ) {
                int16 ux,uy;
                ux=prevod(cond[0]/Uref,15);
                uy=prevod(cond[1]/Uref,15);
                
                int16 yx,yy;
                // zadani mereni
                yx=prevod(val[0]/Iref,15);
                yy=prevod(val[1]/Iref,15);
                
                int32 detS;
                int16 rem;
                ekfCh2(&E, ux,uy,yx,yy, &detS, &rem); //detS,rem asssigned inside
                
                Est._mu()=vec_2(E.x_est[0]*Wref/32768., E.x_est[1]*Thetaref/32768.);
                
                ll = 0.99*ll+( -0.5*(double)rem/32767)*10000-0.0*log((double)detS); // detS in q(8+15+15)-(8+8). - multiplicative differece is not important
        }
        const epdf& posterior() const {return Est;};
        void from_setting ( const Setting &set ){
                BM::from_setting(set);
                vec dQ,dR;
                UI::get ( dQ, set, "dQ", UI::optional );
                UI::get ( dR, set, "dR", UI::optional );
                if (dQ.length()==2){
                        E.Q[0]=prevod(dQ[0],15);      // 1e-3
                        E.Q[3]=prevod(dQ[1],15);      // 1e-3
                }
                if (dR.length()==2){
                        E.dR[0]=prevod(dR[0],15);      // 1e-3
                        E.dR[1]=prevod(dR[1],15);      // 1e-3
                }
        }
        
        enorm<fsqmat> Est;
        mat Ry;
};
UIREGISTER(ekfChfix);



class ekfChfixQ: public BM{
        LOG_LEVEL(ekfChfixQ,logQ,logCh,logC,logRes)
        ekf_data E;
        int64 PSI_Q0, PSI_Q1, PSI_Q0_reg, PSI_Q1_reg;
        int Q_ni;
        int phi_Q;
public:
        ekfChfixQ(){
                init_ekfCh2(&E,0.000125);set_dim(2);    dimc = 2;
                dimy = 2;
                Q_ni = 7;
                phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni);

                PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni;
                PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni;
                PSI_Q0 = ((int64)E.Q[0])<<Q_ni;
                PSI_Q1 = ((int64)E.Q[3])<<Q_ni;
        } 
        void bayes ( const vec &val, const vec &cond ) {
                int16 ux,uy;
                ux=prevod(cond[0]/Uref,15);
                uy=prevod(cond[1]/Uref,15);
                
                int16 yx,yy;
                // zadani mereni
                yx=prevod(val[0]/Iref,15);
                yy=prevod(val[1]/Iref,15);
                
                int16  rem;
                int32 detS;
                ekfCh2(&E, ux,uy,yx,yy, &detS, &rem);
                
                Est._mu()=vec_2(E.x_est[0]*Uref/32768., E.x_est[1]*Uref/32768.);
                
                int16 xerr0 = E.x_est[0]-E.x_pred[0];
                PSI_Q0 = ((int64)(phi_Q*PSI_Q0) + (((int64)xerr0*xerr0)<<15) + PSI_Q0_reg)>>15;         
                E.Q[0] = PSI_Q0>>Q_ni;

                xerr0 = E.x_est[1]-E.x_pred[1];
                PSI_Q1 = ((int64)(phi_Q*PSI_Q1) + (((int64)xerr0*xerr0)<<15) + PSI_Q1_reg)>>15;         
                E.Q[3] = PSI_Q1>>Q_ni;
                
                cout << E.Q[0] << "," << E.Q[3] <<endl;
                ll = -0.5*qlog(detS)-0.5*rem;
        }
        const epdf& posterior() const {return Est;};
        void from_setting ( const Setting &set ){
                BM::from_setting(set);
                vec dQ,dR;
                UI::get ( dQ, set, "dQ", UI::optional );
                UI::get ( dR, set, "dR", UI::optional );
                if (dQ.length()==2){
                        E.Q[0]=prevod(dQ[0],15);      // 1e-3
                        E.Q[3]=prevod(dQ[1],15);      // 1e-3
                }
                if (dR.length()==2){
                        E.dR[0]=prevod(dR[0],15);      // 1e-3
                        E.dR[1]=prevod(dR[1],15);      // 1e-3
                }

                UI::get(Q_ni, set, "Q_ni", UI::optional);
                { // zmena Q!!
                        phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni);

                        PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni;
                        PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni;
                        PSI_Q0 = ((int64)E.Q[0])<<Q_ni;
                        PSI_Q1 = ((int64)E.Q[3])<<Q_ni;

                }
                UI::get(log_level, set, "log_level", UI::optional);

        }
        void log_register ( logger &L, const string &prefix ) {
                BM::log_register(L,prefix);
                L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(2)), prefix);
                L.add_vector(log_level, logCh, RV("{Ch }",vec_1<int>(3)), prefix);
                L.add_vector(log_level, logC, RV("{C }",vec_1<int>(4)), prefix);
                L.add_vector(log_level, logRes, RV("{dy }",vec_1<int>(2)), prefix);
        }
        void log_write() const {
                BM::log_write();
                if ( log_level[logQ] ) {
                        log_level.store( logQ, vec_2((double)E.Q[0],(double)E.Q[3] ));
                }
                if ( log_level[logCh] ) {
                        log_level.store( logCh, vec_3((double)E.Chf[0],(double)E.Chf[1], (double)E.Chf[3]));
                }
                if ( log_level[logRes] ) {
                        log_level.store( logRes, vec_2((double)E.difz[0],(double)E.difz[1]));
                }
                if ( log_level[logC] ) {
                        vec v(4);
                        for (int i=0;i<4;i++) v(i)=E.C[i];
                        log_level.store( logC, v);
                }
        }
        
        enorm<fsqmat> Est;
        mat Ry;
};
UIREGISTER(ekfChfixQ);

#endif // KF_H