Changeset 1464 for applications

Timestamp:

07/23/12 11:03:24 (12 years ago)

Author:

smidl

Message:

upravy choleskiho + nove qmath

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• CMakeLists.txt (modified) (1 diff)
• RArandn.h (added)
• ekf_mm.cpp (added)
• ekf_mm.h (added)
• ekf_obj.cpp (modified) (1 diff)
• ekf_obj.h (modified) (4 diffs)
• matrix_vs.cpp (modified) (4 diffs)
• matrix_vs.h (modified) (1 diff)
• qmath.cpp (added)
• qmath.h (added)

applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt

@@ -6 +6 @@
 
 include_directories(../../bdm)
-add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp mpf_double.cpp)
+add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp mpf_double.cpp ekf_mm.cpp qmath.cpp)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -1076 +1076 @@
         int16 dR[2];dR[0]=R[0];dR[1]=R[3];
         //int16 xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
-        carlson_fastC(difz,x_est,Chf,C,dR,2,2);
+        int16 detS, rem;
+        detS = 32767; rem=0;
+        carlson_fastC(difz,x_est,Chf,C,dR,2,2, &detS, &rem);
         //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
         
+        log_level.store(logDet,vec_1((double)detS));
+        log_level.store(logRem,vec_1((double)rem));
+
         // navrat estimovanych hodnot regulatoru
         vec& mu = E._mu();

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -23 +23 @@
 #include "mpf_double.h"
 #include "fast_exp.h"
+#include "ekf_mm.h"
+#include "qmath.h"
 
 using namespace bdm;
@@ -413 +415 @@
 class EKFfixedCh2 : public BM {
 public:
-        LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP);
+        LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP, logDet, logRem);
         
         void init_ekf2(double Tv);
@@ -470 +472 @@
                 L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
                 L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
-                
-        };
-        //void from_setting();
+                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
+                }
+        }
 };
 
@@ -711 +728 @@
 
 
+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);
+                
+                int16 detS, rem;
+                ekfCh2(&E, ux,uy,yx,yy, &detS, &rem);
+                
+                Est._mu()=vec_2(E.x_est[0]*Uref/32768., E.x_est[1]*Uref/32768.);
+                
+                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 ( dQ, 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);
+
 #endif // KF_H
 

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -663 +663 @@
 }
 
-void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) {
+void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx, int16 *detS, int16 *rem ) {
         int16 alpha,beta,gamma;
         int16 delta, eta,epsilon,zeta,sigma,tau;
@@ -672 +672 @@
         int16 *Ch_ij, *w_i, *x_i, *C_yi;
         
-        
+        *detS = 32767;
         for (iy=0; iy<dimy; iy++)
         {
@@ -695 +695 @@
                         alpha=(tmp_long+(1<<14))>>15;                           // vyssi presnost
                                         
-                        //            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
+                        //gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
                         gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC
                                // in q15
@@ -734 +734 @@
                         w_i++;
                 }
-        }
-}
-
+                
+                *detS = ((int32)*detS*alpha+(1<<14))>>8;
+                *rem += ((int32)delta*delta)/alpha;
+        }
+}
+

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -52 +52 @@
 extern void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
 
-/* perform Givens update of Ch matrix using PSI*Ch , Q, */
+/* perform Givens update of Ch matrix using PSI*Ch and Q, */
 extern void givens_fast(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx);
 
-/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
+/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx, asuming identity matrix C*/
 extern void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
 
 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
-extern void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
+extern void carlson_fastC(int16* difz, int16* xp, int16* Ch, int16* C, int16* R, unsigned int16 dimy, unsigned int16 dimx, int16* detS, int16* rem );