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

Revision 1264, 8.5 kB (checked in by smidl, 14 years ago)

experiments

  • Property svn:eol-style set to native
Line 
1/*!
2  \file
3  \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
4  \author Vaclav Smidl.
5
6  -----------------------------------
7  BDM++ - C++ library for Bayesian Decision Making under Uncertaint16y
8
9  Using IT++ for numerical operations
10  -----------------------------------
11*/
12
13#ifndef EKFfix_H
14#define EKFfix_H
15
16
17#include <estim/kalman.h>
18#include "fixed.h"
19#include "matrix.h"
20#include "matrix_vs.h"
21#include "reference_Q15.h"
22#include "parametry_motoru.h"
23
24using namespace bdm;
25
26double minQ(double Q);
27
28void mat_to_int16(const imat &M, int16 *I);
29void vec_to_int16(const ivec &v, int16 *I);
30void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref);
31
32#ifdef XXX
33/*!
34\brief Extended Kalman Filter with full matrices in fixed point16 arithmetic
35
36An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
37*/
38class EKFfixed : public BM {
39public:
40void init_ekf(double Tv);
41void ekf(double ux, double uy, double isxd, double isyd);
42
43/* Declaration of local functions */
44void prediction(int16 *ux);
45void correction(void);
46void update_psi(void);
47
48/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
49 int16 Q[16]; /* matrix [4,4] */
50 int16 R[4]; /* matrix [2,2] */
51
52 int16 x_est[4];
53 int16 x_pred[4];
54 int16 P_pred[16]; /* matrix [4,4] */
55 int16 P_est[16]; /* matrix [4,4] */
56 int16 Y_mes[2];
57 int16 ukalm[2];
58 int16 Kalm[8]; /* matrix [5,2] */
59
60 int16 PSI[16]; /* matrix [4,4] */
61
62 int16 temp15a[16];
63
64 int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
65
66 int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
67 enorm<fsqmat> E;
68 mat Ry;
69 
70public:
71        //! Default constructor
72        EKFfixed ():BM(),E(),Ry(2,2){
73                int16 i;
74                for(i=0;i<16;i++){Q[i]=0;}
75                for(i=0;i<4;i++){R[i]=0;}
76
77                for(i=0;i<4;i++){x_est[i]=0;}
78                for(i=0;i<4;i++){x_pred[i]=0;}
79                for(i=0;i<16;i++){P_pred[i]=0;}
80                for(i=0;i<16;i++){P_est[i]=0;}
81                P_est[0]=0x7FFF;
82                P_est[5]=0x7FFF;
83                P_est[10]=0x7FFF;
84                P_est[15]=0x7FFF;
85                for(i=0;i<2;i++){Y_mes[i]=0;}
86                for(i=0;i<2;i++){ukalm[i]=0;}
87                for(i=0;i<8;i++){Kalm[i]=0;}
88
89                for(i=0;i<16;i++){PSI[i]=0;}
90
91                set_dim(4);
92                E._mu()=zeros(4);
93                E._R()=zeros(4,4);
94                init_ekf(0.000125);
95        };
96        //! Here dt = [yt;ut] of appropriate dimensions
97        void bayes ( const vec &yt, const vec &ut );
98        //!dummy!
99        const epdf& posterior() const {return E;};
100       
101};
102
103UIREGISTER(EKFfixed);
104
105#endif
106
107//! EKF for testing q44
108class EKFtest: public EKFfull{
109        void bayes ( const vec &yt, const vec &cond ) {
110                EKFfull::bayes(yt,cond);
111                mat &P = est._R();
112                if (P(3,3)>3.14) 
113                        P(3,3)=3.14;           
114        }
115};
116UIREGISTER(EKFtest);
117
118/*!
119\brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
120
121An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
122*/
123class EKFfixedUD : public BM {
124        public:
125                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP);
126               
127                void init_ekf(double Tv);
128                void ekf(double ux, double uy, double isxd, double isyd);
129                               
130                /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
131                int16 Q[16]; /* matrix [4,4] */
132                int16 R[4]; /* matrix [2,2] */
133               
134                int16 x_est[4]; /* estimate and prediction */
135               
136                int16 PSI[16]; /* matrix [4,4] */
137                int16 PSIU[16]; /* matrix PIS*U, [4,4] */
138               
139                int16 Uf[16]; // upper triangular of covariance (inplace)
140                int16 Df[4];  // diagonal covariance
141                int16 Dfold[4]; // temp of D
142                int16 G[16];  // temp for bierman
143               
144                int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
145               
146                enorm<fsqmat> E;
147                mat Ry;
148               
149        public:
150                //! Default constructor
151                EKFfixedUD ():BM(),E(),Ry(2,2){
152                        int16 i;
153                        for(i=0;i<16;i++){Q[i]=0;}
154                        for(i=0;i<4;i++){R[i]=0;}
155                       
156                        for(i=0;i<4;i++){x_est[i]=0;}
157                        for(i=0;i<16;i++){Uf[i]=0;}
158                        for(i=0;i<4;i++){Df[i]=0;}
159                        for(i=0;i<16;i++){G[i]=0;}
160                        for(i=0;i<4;i++){Dfold[i]=0;}
161                       
162                        for(i=0;i<16;i++){PSI[i]=0;}
163                       
164                        set_dim(4);
165                        E._mu()=zeros(4);
166                        E._R()=zeros(4,4);
167                        init_ekf(0.000125);
168                };
169                //! Here dt = [yt;ut] of appropriate dimensions
170                void bayes ( const vec &yt, const vec &ut );
171                //!dummy!
172                const epdf& posterior() const {return E;};
173                void log_register(logger &L, const string &prefix){
174                        BM::log_register ( L, prefix );
175                       
176                                L.add_vector ( log_level, logG, RV("G",16), prefix );
177                                L.add_vector ( log_level, logU, RV ("U", 16 ), prefix );
178                                L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
179                                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
180                                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
181                               
182                };
183                //void from_setting();
184};
185
186UIREGISTER(EKFfixedUD);
187
188/*!
189 * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic
190 *
191 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
192 */
193class EKFfixedCh : public BM {
194public:
195        LOG_LEVEL(EKFfixedCh,logCh, logA, logP);
196       
197        void init_ekf(double Tv);
198        void ekf(double ux, double uy, double isxd, double isyd);
199       
200        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
201        int16 Q[16]; /* matrix [4,4] */
202        int16 R[4]; /* matrix [2,2] */
203       
204        int16 x_est[4]; /* estimate and prediction */
205       
206        int16 PSI[16]; /* matrix [4,4] */
207        int16 PSICh[16]; /* matrix PIS*U, [4,4] */
208       
209        int16 Chf[16]; // upper triangular of covariance (inplace)
210       
211        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
212       
213        enorm<chmat> E;
214        mat Ry;
215       
216public:
217        //! Default constructor
218        EKFfixedCh ():BM(),E(),Ry(2,2){
219                int16 i;
220                for(i=0;i<16;i++){Q[i]=0;}
221                for(i=0;i<4;i++){R[i]=0;}
222               
223                for(i=0;i<4;i++){x_est[i]=0;}
224                for(i=0;i<16;i++){Chf[i]=0;}
225               
226                for(i=0;i<16;i++){PSI[i]=0;}
227               
228                set_dim(4);
229                E._mu()=zeros(4);
230                E._R()=zeros(4,4);
231                init_ekf(0.000125);
232        };
233        //! Here dt = [yt;ut] of appropriate dimensions
234        void bayes ( const vec &yt, const vec &ut );
235        //!dummy!
236        const epdf& posterior() const {return E;};
237        void log_register(logger &L, const string &prefix){
238                BM::log_register ( L, prefix );
239               
240                L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix );
241                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
242                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
243               
244        };
245        //void from_setting();
246};
247
248UIREGISTER(EKFfixedCh);
249
250
251//! EKF for comparison of EKF_UD with its fixed-point16 implementation
252class EKF_UDfix : public BM {
253        protected:
254                //! logger
255                LOG_LEVEL(EKF_UDfix,logU, logG);
256                //! Internal Model f(x,u)
257                shared_ptr<diffbifn> pfxu;
258               
259                //! Observation Model h(x,u)
260                shared_ptr<diffbifn> phxu;
261               
262                //! U part
263                mat U;
264                //! D part
265                vec D;
266                               
267                mat A;
268                mat C;
269                mat Q;
270                vec R;
271               
272                enorm<ldmat> est;
273               
274               
275        public:
276               
277                //! copy constructor duplicated
278                EKF_UDfix* _copy() const {
279                        return new EKF_UDfix(*this);
280                }
281               
282                const enorm<ldmat>& posterior()const{return est;};
283               
284                enorm<ldmat>& prior() {
285                        return const_cast<enorm<ldmat>&>(posterior());
286                }
287               
288                EKF_UDfix(){}
289               
290               
291                EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){}
292               
293                //! Set nonlinear functions for mean values and covariance matrices.
294                void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 );
295               
296                //! Here dt = [yt;ut] of appropriate dimensions
297                void bayes ( const vec &yt, const vec &cond = empty_vec );
298               
299                void log_register ( bdm::logger& L, const string& prefix ){
300                        BM::log_register ( L, prefix );
301                       
302                        if ( log_level[logU] )
303                                L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix );
304                        if ( log_level[logG] )
305                                L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix );
306                       
307                }
308                /*! Create object from the following structure
309               
310                \code
311                class = 'EKF_UD';
312                OM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
313                IM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
314                dQ = [...];                             % vector containing diagonal of Q
315                dR = [...];                             % vector containing diagonal of R
316                --- optional fields ---
317                mu0 = [...];                            % vector of statistics mu0
318                dP0 = [...];                            % vector containing diagonal of P0
319                -- or --
320                P0 = [...];                             % full matrix P0
321                --- inherited fields ---
322                bdm::BM::from_setting
323                \endcode
324                If the optional fields are not given, they will be filled as follows:
325                \code
326                mu0 = [0,0,0,....];                     % empty statistics
327                P0 = eye( dim );             
328                \endcode
329                */
330                void from_setting ( const Setting &set );
331               
332                void validate() {};
333                // TODO dodelat void to_setting( Setting &set ) const;
334               
335};
336UIREGISTER(EKF_UDfix);
337
338
339
340
341#endif // KF_H
342
Note: See TracBrowser for help on using the browser.