Show
Ignore:
Timestamp:
09/17/10 11:47:39 (14 years ago)
Author:
smidl
Message:

stress

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • library/tests/stresssuite/kalman_stress.cpp

    r1158 r1190  
    103103    vec dt; 
    104104    tt.tic(); 
     105    vec mu=mu0; 
     106    mat iRy; 
     107    mat Ry; 
     108    mat P=P0; 
     109    mat K; 
     110    vec ut; 
     111    vec yt; 
    105112    for ( int t = 1; t < Ndat; t++ ) { 
    106113        dt = Dt.get_col ( t ); 
    107         KFu.bayes ( dt.get ( 0, C.rows() - 1 ), dt.get ( C.rows(), dt.length() - 1 ) ); 
    108         Xtu.set_col ( t, KFuep.mean() ); 
     114        yt= dt.get ( 0, C.rows() - 1 ); 
     115        ut = dt.get ( C.rows(), dt.length() - 1 ) ; 
     116         
     117        mu = A*mu + B*ut; 
     118        P  = A*P*A.T() + Q; 
     119 
     120        //Data update 
     121        Ry = C*P*C.T() + R; 
     122        iRy = inv(Ry); 
     123        K = P*C.T()*iRy;  
     124        P = P- K*C*P; // P = P -KCP; 
     125        mu = mu + K*(yt-C*mu-D*ut); 
     126        Xtu.set_col ( t, KFuep.mean() ); 
    109127    } 
    110128    exec_times ( 0 ) = tt.toc();