mtt: /home/laradmin/lar/perception/planarobstacles/mtt/src/mtt

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00032 #include <mtt/mtt_kalman.h>
00033 
00034 #define CV_MODEL 0
00035 
00036 int select_object;
00037 extern double gl;
00038 
00039 
00040 void AddPointErrorVectors(t_errors*error,double x_inno,double y_inno,double x_resi,double y_resi,double lateral_error)
00041 {
00042         error->x_innovation[error->position]=x_inno;
00043         error->y_innovation[error->position]=y_inno;
00044         
00045         error->x_residue[error->position]=x_resi;
00046         error->y_residue[error->position]=y_resi;
00047         
00048         error->lateral_error[error->position]=lateral_error;
00049         
00050         error->latest=error->position;
00051         error->position++;
00052         
00053         GetErrorConvariance(error);
00054         
00055         if(error->position==error->max_number_points)
00056                 error->position=0;
00057         
00058         error->next=error->position;
00059         
00060         if(error->number_points<error->max_number_points)
00061                 error->number_points++;
00062 }
00063 
00064 void MotionModelsIteration(vector<t_listPtr> &list,t_config& config)
00065 {
00066         CvMat*x_measurement=cvCreateMat( 1, 1, CV_32FC1 );
00067         CvMat*y_measurement=cvCreateMat( 1, 1, CV_32FC1 );
00068         
00069         double x_estimated_last=0, y_estimated_last=0;
00070         
00071         static bool initialise=TRUE;
00072         static FILE*fp;
00073         if(initialise)
00074         {
00075                 fp=fopen("data","w");
00076                 if(!fp)
00077                 {
00078                         perror("Open data");
00079                         printf("Cannot save objects data to disk\nPlease do not chose an object it will cause a segmentation fault\n");
00080                 }
00081                 
00082                 initialise=FALSE;
00083         }
00084         
00085         for(uint i=0;i<list.size();i++)
00086         {
00087                 
00089                 if(list[i]->timers.lifetime==0)
00090                 {
00091                         cvSetReal2D(list[i]->motion_models.cv_x_kalman->state_pre,0,0,list[i]->measurements.x);
00092                         cvSetReal2D(list[i]->motion_models.cv_y_kalman->state_pre,0,0,list[i]->measurements.y);
00093                         
00094                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->state_pre,0,0,list[i]->measurements.x);
00095                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->state_pre,0,0,list[i]->measurements.y);
00096                         
00097                         cvSetReal2D(list[i]->motion_models.cv_x_kalman->state_post,0,0,list[i]->measurements.x);
00098                         cvSetReal2D(list[i]->motion_models.cv_y_kalman->state_post,0,0,list[i]->measurements.y);
00099                         
00100                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->state_post,0,0,list[i]->measurements.x);
00101                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->state_post,0,0,list[i]->measurements.y);
00102                 }else
00103                 {
00105                         
00106                         cvSetReal2D(x_measurement,0,0,list[i]->measurements.x);
00107                         cvSetReal2D(y_measurement,0,0,list[i]->measurements.y); 
00108                         
00109                         x_estimated_last=list[i]->position.estimated_x;
00110                         y_estimated_last=list[i]->position.estimated_y;
00111                         
00112                         cvKalmanCorrect(list[i]->motion_models.cv_x_kalman,x_measurement);
00113                         cvKalmanCorrect(list[i]->motion_models.cv_y_kalman,y_measurement);
00114                         
00115                         cvKalmanCorrect(list[i]->motion_models.ca_x_kalman,x_measurement);
00116                         cvKalmanCorrect(list[i]->motion_models.ca_y_kalman,y_measurement);
00117                 }
00118                 
00119                 if((int)list[i]->id==select_object)
00120                         fprintf(fp,"%2.6f %2.6f ",list[i]->measurements.x,list[i]->measurements.y);
00121                 
00123                 
00124                 double cv_pestimated_x=cvGetReal2D(list[i]->motion_models.cv_x_kalman->state_post,0,0);
00125                 double cv_pestimated_y=cvGetReal2D(list[i]->motion_models.cv_y_kalman->state_post,0,0);
00126                 
00127                 double ca_pestimated_x=cvGetReal2D(list[i]->motion_models.ca_x_kalman->state_post,0,0);
00128                 double ca_pestimated_y=cvGetReal2D(list[i]->motion_models.ca_y_kalman->state_post,0,0);
00129                 
00130                 if((int)list[i]->id==select_object)
00131                         fprintf(fp,"%2.6f %2.6f %2.6f %2.6f ",cv_pestimated_x,cv_pestimated_y,ca_pestimated_x,ca_pestimated_y);
00132                 
00134                 cvKalmanPredict(list[i]->motion_models.cv_x_kalman);
00135                 cvKalmanPredict(list[i]->motion_models.cv_y_kalman);
00136                 
00137                 cvKalmanPredict(list[i]->motion_models.ca_x_kalman);
00138                 cvKalmanPredict(list[i]->motion_models.ca_y_kalman);
00139                 
00141                 
00142                 double cv_ppredicted_x=cvGetReal2D(list[i]->motion_models.cv_x_kalman->state_pre,0,0);
00143                 double cv_ppredicted_y=cvGetReal2D(list[i]->motion_models.cv_y_kalman->state_pre,0,0);
00144                 
00145                 double ca_ppredicted_x=cvGetReal2D(list[i]->motion_models.ca_x_kalman->state_pre,0,0);
00146                 double ca_ppredicted_y=cvGetReal2D(list[i]->motion_models.ca_y_kalman->state_pre,0,0);
00147                 
00148                 if((int)list[i]->id==select_object)
00149                         fprintf(fp,"%2.6f %2.6f %2.6f %2.6f ",cv_ppredicted_x,cv_ppredicted_y,ca_ppredicted_x,ca_ppredicted_y);
00150                 
00151                 //              CvPoint a=cvPoint(real2print(x_ca_pre,config),real2print(y_ca_pre,config));
00152                 
00153                 //              cvLine(img,a,a,CV_RGB(0,255,0),2, 8, 0 );
00154                 
00156                 
00157                 double cv_inno_x=list[i]->measurements.x-cv_ppredicted_x;
00158                 double cv_inno_y=list[i]->measurements.y-cv_ppredicted_y;
00159                 
00160                 double ca_inno_x=list[i]->measurements.x-ca_ppredicted_x;
00161                 double ca_inno_y=list[i]->measurements.y-ca_ppredicted_y;
00162                 
00163                 double cv_resi_x=list[i]->measurements.x-cv_pestimated_x;
00164                 double cv_resi_y=list[i]->measurements.y-cv_pestimated_y;
00165                 
00166                 double ca_resi_x=list[i]->measurements.x-ca_pestimated_x;
00167                 double ca_resi_y=list[i]->measurements.y-ca_pestimated_y;
00168                 
00169                 //              double scvi=sqrt(pow(cv_inno_x,2)+pow(cv_inno_y,2));
00170                 //              double scai=sqrt(pow(ca_inno_x,2)+pow(ca_inno_y,2));
00171                 
00172                 if((int)list[i]->id==select_object)
00173                         fprintf(fp,"%2.6f %2.6f %2.6f %2.6f ",cv_inno_x,cv_inno_y,ca_inno_x,ca_inno_y);
00174                 
00175                 if((int)list[i]->id==select_object)
00176                         fprintf(fp,"%2.6f %2.6f %2.6f %2.6f ",cv_resi_x,cv_resi_y,ca_resi_x,ca_resi_y);
00177                 
00178                 //              if(scvi<scai)
00179                 //                      list[i]->model=CV;
00180                 //              else
00181                 
00182                 //              list[i]->model=CA;
00183                 //              if(list[i]->timers.lifetime<30)
00184                 //                      list[i]->model=CV;
00185                 // //                   
00186                 //                      list[i]->model=MIX;
00187                 
00188                 //              if(list[i]->classification.occluded)
00189                 //                      list[i]->model=CV;
00190                 
00191                 //              list[i]->model=CA;
00192                 list[i]->model=CV;
00193                 
00194                 switch(list[i]->model)
00195                 {
00196                         case CV:
00197                                 list[i]->position.estimated_x=cv_pestimated_x;
00198                                 list[i]->position.estimated_y=cv_pestimated_y;
00199                                 list[i]->position.predicted_x=cv_ppredicted_x;
00200                                 list[i]->position.predicted_y=cv_ppredicted_y;
00201                                 break;
00202                         case CA:
00203                                 list[i]->position.estimated_x=ca_pestimated_x;
00204                                 list[i]->position.estimated_y=ca_pestimated_y;
00205                                 list[i]->position.predicted_x=ca_ppredicted_x;
00206                                 list[i]->position.predicted_y=ca_ppredicted_y;
00207                                 break;
00208                         case MIX:
00209                                 list[i]->position.estimated_x=(ca_pestimated_x+cv_pestimated_x)/2;
00210                                 list[i]->position.estimated_y=(ca_pestimated_y+cv_pestimated_y)/2;
00211                                 list[i]->position.predicted_x=(ca_ppredicted_x+cv_ppredicted_x)/2;
00212                                 list[i]->position.predicted_y=(ca_ppredicted_y+cv_ppredicted_y)/2;
00213                                 break;
00214                 }
00215                 
00217                 AddPointPath(&(list[i]->path_cv),list[i]->position.estimated_x,list[i]->position.estimated_y);
00218                 AddPointPath(&(list[i]->path_ca),list[i]->position.estimated_x,list[i]->position.estimated_y);
00219                 
00221                 if((int)list[i]->id==select_object)
00222                 {
00223                         double cvvx,cvvy,cavx,cavy;
00224                         cvvx=cvGetReal2D(list[i]->motion_models.cv_x_kalman->state_post,1,0);
00225                         cvvy=cvGetReal2D(list[i]->motion_models.cv_y_kalman->state_post,1,0);
00226                         cavx=cvGetReal2D(list[i]->motion_models.ca_x_kalman->state_post,1,0);
00227                         cavy=cvGetReal2D(list[i]->motion_models.ca_y_kalman->state_post,1,0);
00228                         
00229                         fprintf(fp,"%2.6f %2.6f %2.6f %2.6f ",cvvx,cvvy,cavx,cavy);
00230                 }
00231                 
00233                 
00234                 double lateral_error;
00235                 
00236                 switch(list[i]->model)
00237                 {
00238                         case CV:
00239                                 list[i]->velocity.velocity_x=cvGetReal2D(list[i]->motion_models.cv_x_kalman->state_post,1,0);
00240                                 list[i]->velocity.velocity_y=cvGetReal2D(list[i]->motion_models.cv_y_kalman->state_post,1,0);
00241                                 break;
00242                         case CA:
00243                                 list[i]->velocity.velocity_x=cvGetReal2D(list[i]->motion_models.ca_x_kalman->state_post,1,0);
00244                                 list[i]->velocity.velocity_y=cvGetReal2D(list[i]->motion_models.ca_y_kalman->state_post,1,0);
00245                                 break;
00246                         default:
00247                                 list[i]->velocity.velocity_x=cvGetReal2D(list[i]->motion_models.cv_x_kalman->state_post,1,0);
00248                                 list[i]->velocity.velocity_y=cvGetReal2D(list[i]->motion_models.cv_y_kalman->state_post,1,0);
00249                                 break;
00250                 }
00251                 
00252                 list[i]->velocity.velocity_module=sqrt(pow(list[i]->velocity.velocity_x,2)+pow(list[i]->velocity.velocity_y,2));
00253                 list[i]->velocity.velocity_angle=atan2(-list[i]->velocity.velocity_y,list[i]->velocity.velocity_x);
00254                 
00255                 double xi,xf,yi,yf;
00256                 
00257                 xi=x_estimated_last;
00258                 yi=y_estimated_last;
00259                 xf=xi+list[i]->velocity.velocity_x;
00260                 yf=yi+list[i]->velocity.velocity_y;
00261                 
00262                 double alpha=atan2(xi-xf,yf-yi)+M_PI;
00263                 double ro=xi*cos(alpha)+yi*sin(alpha);
00264                 
00265                 double x_m=cvGetReal2D(x_measurement,0,0);
00266                 double y_m=cvGetReal2D(y_measurement,0,0);
00267                 
00268                 lateral_error = point2line_distance(alpha,ro,x_m,y_m);
00269                 
00270                 AddPointErrorVectors(&(list[i]->errors_cv),cv_inno_x,cv_inno_y,cv_resi_x,cv_resi_y,lateral_error);
00271                 AddPointErrorVectors(&(list[i]->errors_ca),ca_inno_x,ca_inno_y,ca_resi_x,ca_resi_y,lateral_error);
00272                 
00274                 if((int)list[i]->id==select_object)
00275                         fprintf(fp,"%2.6f ",lateral_error);
00276                 
00278                 if((int)list[i]->id==select_object)
00279                 {
00280                         double cvcovx,cvcovy,cacovx,cacovy;
00281                         cvcovx=list[i]->errors_cv.x_inno_cov;
00282                         cvcovy=list[i]->errors_cv.y_inno_cov;
00283                         cacovx=list[i]->errors_ca.x_inno_cov;
00284                         cacovy=list[i]->errors_ca.y_inno_cov;
00285                         
00286                         fprintf(fp,"%2.6f %2.6f %2.6f %2.6f ",cvcovx,cvcovy,cacovx,cacovy);
00287                         
00288                         double cvgpx,cvgpy,cvgvx,cvgvy,cagpx,cagpy,cagvx,cagvy;
00289                         cvgpx=cvGetReal2D(list[i]->motion_models.cv_x_kalman->gain,0,0);
00290                         cvgpy=cvGetReal2D(list[i]->motion_models.cv_y_kalman->gain,0,0);
00291                         cvgvx=cvGetReal2D(list[i]->motion_models.cv_x_kalman->gain,1,0);
00292                         cvgvy=cvGetReal2D(list[i]->motion_models.cv_y_kalman->gain,1,0);
00293                         
00294                         cagpx=cvGetReal2D(list[i]->motion_models.ca_x_kalman->gain,0,0);
00295                         cagpy=cvGetReal2D(list[i]->motion_models.ca_y_kalman->gain,0,0);
00296                         cagvx=cvGetReal2D(list[i]->motion_models.ca_x_kalman->gain,1,0);
00297                         cagvy=cvGetReal2D(list[i]->motion_models.ca_y_kalman->gain,1,0);
00298                         
00299                         fprintf(fp,"%2.6f %2.6f %2.6f %2.6f %2.6f %2.6f %2.6f %2.6f ",cvgpx,cvgpy,cvgvx,cvgvy,cagpx,cagpy,cagvx,cagvy);
00300                 }
00301                 
00302                 double previous_factor=1;
00303                 
00304                 if(list[i]->classification.velocity_classification==STATIONARY)
00305                         previous_factor=2;
00306                 else if(list[i]->classification.velocity_classification==MOVING)
00307                         previous_factor=1./2.;
00308                 
00309                 if(list[i]->velocity.velocity_module < config.max_stationary_velocity*previous_factor)
00310                 {
00311                         list[i]->classification.velocity_classification=STATIONARY;
00312                         //                      printf("Id %d Stationary\n",objects[i]->id);
00313                 }
00314                 if(list[i]->velocity.velocity_module > config.min_moving_velocity*previous_factor || list[i]->timers.lifetime<5)
00315                 {
00316                         list[i]->classification.velocity_classification=MOVING;
00317                         //                      printf("Id %d Moving\n",objects[i]->id);        
00318                 }
00319                 
00321                 
00322                 SetSearchArea(*list[i],config);
00323                 
00325                 
00326                 if(list[i]->classification.velocity_classification==STATIONARY)
00327                 {
00328                         cvSetIdentity( list[i]->motion_models.cv_x_kalman->measurement_noise_cov, cvRealScalar(0.1*0.1));
00329                         cvSetIdentity( list[i]->motion_models.cv_y_kalman->measurement_noise_cov, cvRealScalar(0.1*0.1));
00330                         
00331                         cvSetIdentity( list[i]->motion_models.ca_x_kalman->measurement_noise_cov, cvRealScalar(0.1*0.1));
00332                         cvSetIdentity( list[i]->motion_models.ca_y_kalman->measurement_noise_cov, cvRealScalar(0.1*0.1));
00333                 }else
00334                 {
00335                         cvSetIdentity( list[i]->motion_models.cv_x_kalman->measurement_noise_cov, cvRealScalar(0.05*0.05));
00336                         cvSetIdentity( list[i]->motion_models.cv_y_kalman->measurement_noise_cov, cvRealScalar(0.05*0.05));
00337                         
00338                         cvSetIdentity( list[i]->motion_models.ca_x_kalman->measurement_noise_cov, cvRealScalar(0.05*0.05));
00339                         cvSetIdentity( list[i]->motion_models.ca_y_kalman->measurement_noise_cov, cvRealScalar(0.05*0.05));
00340                 }
00341                 
00343                 double xAcv=sqrt(list[i]->errors_cv.x_inno_cov);
00344                 double yAcv=sqrt(list[i]->errors_cv.y_inno_cov);
00345                 double xAca=sqrt(list[i]->errors_ca.x_inno_cov);
00346                 double yAca=sqrt(list[i]->errors_ca.y_inno_cov);
00347                 
00348                 if(xAcv<0.001)xAcv=0.001;
00349                 if(yAcv<0.001)yAcv=0.001;
00350                 
00351                 if(xAcv>2)xAcv=2;
00352                 if(yAcv>2)yAcv=2;
00353                 
00354                 if(xAca<0.001)xAca=0.001;
00355                 if(yAca<0.001)yAca=0.001;
00356                 
00357                 if(xAca>2)xAca=2;
00358                 if(yAca>2)yAca=2;
00359                 
00360                 double dt=config.dt;
00361                 //                      printf("Id %d xA %2.2f yA %2.2f\n",objects[i]->id,xA,yA);
00362                 
00363                 if(list[i]->classification.occluded==FALSE)
00364                 {
00365                         double xQcv[]={pow(xAcv,2)*pow(dt,3)/3,pow(xAcv,2)*pow(dt,2)/2,pow(xAcv,2)*pow(dt,2)/2,pow(xAcv,2)*pow(dt,1)};
00366                         double yQcv[]={pow(yAcv,2)*pow(dt,3)/3,pow(yAcv,2)*pow(dt,2)/2,pow(yAcv,2)*pow(dt,2)/2,pow(yAcv,2)*pow(dt,1)};
00367                         
00368                         double xQca[]={pow(xAca,2)*pow(dt,5)/20, pow(xAca,2)*pow(dt,4)/8, pow(xAca,2)*pow(dt,3)/6,
00369                         pow(xAca,2)*pow(dt,4)/8, pow(xAca,2)*pow(dt,3)/3,  pow(xAca,2)*pow(dt,2)/2,
00370                         pow(xAca,2)*pow(dt,3)/6,  pow(xAca,2)*pow(dt,2)/2,  pow(xAca,2)*pow(dt,1)/1};
00371                         
00372                         double yQca[]={pow(yAca,2)*pow(dt,5)/20, pow(yAca,2)*pow(dt,4)/8, pow(yAca,2)*pow(dt,3)/6,
00373                         pow(yAca,2)*pow(dt,4)/8, pow(yAca,2)*pow(dt,3)/3,  pow(yAca,2)*pow(dt,2)/2,
00374                         pow(yAca,2)*pow(dt,3)/6,  pow(yAca,2)*pow(dt,2)/2,  pow(yAca,2)*pow(dt,1)/1};
00375                         
00376                         cvSetReal2D(list[i]->motion_models.cv_x_kalman->process_noise_cov, 0,0,xQcv[0] );
00377                         cvSetReal2D(list[i]->motion_models.cv_x_kalman->process_noise_cov, 0,1,xQcv[1] );
00378                         cvSetReal2D(list[i]->motion_models.cv_x_kalman->process_noise_cov, 1,0,xQcv[2] );
00379                         cvSetReal2D(list[i]->motion_models.cv_x_kalman->process_noise_cov, 1,1,xQcv[3] );
00380                         
00381                         cvSetReal2D(list[i]->motion_models.cv_y_kalman->process_noise_cov, 0,0,yQcv[0] );
00382                         cvSetReal2D(list[i]->motion_models.cv_y_kalman->process_noise_cov, 0,1,yQcv[1] );
00383                         cvSetReal2D(list[i]->motion_models.cv_y_kalman->process_noise_cov, 1,0,yQcv[2] );
00384                         cvSetReal2D(list[i]->motion_models.cv_y_kalman->process_noise_cov, 1,1,yQcv[3] );
00385                         
00386                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 0,0,xQca[0] );
00387                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 0,1,xQca[1] );
00388                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 0,2,xQca[2] );
00389                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 1,0,xQca[3] );
00390                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 1,1,xQca[4] );
00391                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 1,2,xQca[5] );
00392                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 2,0,xQca[6] );
00393                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 2,1,xQca[7] );
00394                         cvSetReal2D(list[i]->motion_models.ca_x_kalman->process_noise_cov, 2,2,xQca[8] );
00395                         
00396                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 0,0,yQca[0] );
00397                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 0,1,yQca[1] );
00398                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 0,2,yQca[2] );
00399                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 1,0,yQca[3] );
00400                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 1,1,yQca[4] );
00401                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 1,2,yQca[5] );
00402                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 2,0,yQca[6] );
00403                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 2,1,yQca[7] );
00404                         cvSetReal2D(list[i]->motion_models.ca_y_kalman->process_noise_cov, 2,2,yQca[8] );
00405                 }
00406                 
00407                 if((int)list[i]->id==select_object)
00408                         fprintf(fp,"\n");
00409         }
00410         
00411         cvReleaseMat(&(x_measurement));
00412         cvReleaseMat(&(y_measurement));
00413 }
00414 
00415 void AllocMotionModels(t_list&list,t_config&config)
00416 {
00417         list.motion_models.cv_x_kalman = cvCreateKalman( 2, 1, 0 );
00418         list.motion_models.cv_y_kalman = cvCreateKalman( 2, 1, 0 );
00419         
00420         list.motion_models.ca_x_kalman = cvCreateKalman( 3, 1, 0 );
00421         list.motion_models.ca_y_kalman = cvCreateKalman( 3, 1, 0 );
00422         
00423         double a=20;
00424         double dt=config.dt;
00425         
00426         float Acv[] = { 1, dt,
00427         0, 1 };
00428         
00429         float Aca[] = { 1,    dt,  0.5*pow(dt,2),
00430         0,     1,             dt,
00431         0,     0,              1 };
00432         
00433         double Qcv[]={a*a*pow(dt,3)/3,  a*a*pow(dt,2)/2,
00434         a*a*pow(dt,2)/2,  a*a*pow(dt,1)};
00435         
00436         double Qca[]={pow(a,2)*pow(dt,5)/20, pow(a,2)*pow(dt,4)/8, pow(a,2)*pow(dt,3)/6,
00437         pow(a,2)*pow(dt,4)/8, pow(a,2)*pow(dt,3)/3,  pow(a,2)*pow(dt,2)/2,
00438         pow(a,2)*pow(dt,3)/6,  pow(a,2)*pow(dt,2)/2,  pow(a,2)*pow(dt,1)/1};
00439         
00440         memcpy(list.motion_models.cv_x_kalman->transition_matrix->data.fl, Acv, sizeof(Acv));
00441         memcpy(list.motion_models.cv_y_kalman->transition_matrix->data.fl, Acv, sizeof(Acv));
00442         
00443         memcpy(list.motion_models.ca_x_kalman->transition_matrix->data.fl, Aca, sizeof(Aca));
00444         memcpy(list.motion_models.ca_y_kalman->transition_matrix->data.fl, Aca, sizeof(Aca));
00445         
00446         cvSetIdentity( list.motion_models.cv_x_kalman->measurement_matrix, cvRealScalar(1) );
00447         cvSetIdentity( list.motion_models.cv_y_kalman->measurement_matrix, cvRealScalar(1) );
00448         
00449         cvSetIdentity( list.motion_models.ca_x_kalman->measurement_matrix, cvRealScalar(1) );
00450         cvSetIdentity( list.motion_models.ca_y_kalman->measurement_matrix, cvRealScalar(1) );
00451         
00452         cvSetIdentity( list.motion_models.cv_x_kalman->measurement_noise_cov, cvRealScalar(0.04*0.04));
00453         cvSetIdentity( list.motion_models.cv_x_kalman->error_cov_post, cvRealScalar(1));
00454         
00455         cvSetIdentity( list.motion_models.ca_x_kalman->measurement_noise_cov, cvRealScalar(0.04*0.04));
00456         cvSetIdentity( list.motion_models.ca_x_kalman->error_cov_post, cvRealScalar(1));
00457         
00458         cvSetIdentity( list.motion_models.cv_y_kalman->measurement_noise_cov, cvRealScalar(0.04*0.04));
00459         cvSetIdentity( list.motion_models.cv_y_kalman->error_cov_post, cvRealScalar(1));
00460         
00461         cvSetIdentity( list.motion_models.ca_y_kalman->measurement_noise_cov, cvRealScalar(0.04*0.04));
00462         cvSetIdentity( list.motion_models.ca_y_kalman->error_cov_post, cvRealScalar(1));
00463         
00464         cvSetReal2D(list.motion_models.cv_x_kalman->process_noise_cov, 0,0,Qcv[0] );
00465         cvSetReal2D(list.motion_models.cv_x_kalman->process_noise_cov, 0,1,Qcv[1] );
00466         cvSetReal2D(list.motion_models.cv_x_kalman->process_noise_cov, 1,0,Qcv[2] );
00467         cvSetReal2D(list.motion_models.cv_x_kalman->process_noise_cov, 1,1,Qcv[3] );
00468         
00469         cvSetReal2D(list.motion_models.cv_y_kalman->process_noise_cov, 0,0,Qcv[0] );
00470         cvSetReal2D(list.motion_models.cv_y_kalman->process_noise_cov, 0,1,Qcv[1] );
00471         cvSetReal2D(list.motion_models.cv_y_kalman->process_noise_cov, 1,0,Qcv[2] );
00472         cvSetReal2D(list.motion_models.cv_y_kalman->process_noise_cov, 1,1,Qcv[3] );
00473         
00474         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 0,0,Qca[0] );
00475         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 0,1,Qca[1] );
00476         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 0,2,Qca[2] );
00477         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 1,0,Qca[3] );
00478         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 1,1,Qca[4] );
00479         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 1,2,Qca[5] );
00480         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 2,0,Qca[6] );
00481         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 2,1,Qca[7] );
00482         cvSetReal2D(list.motion_models.ca_x_kalman->process_noise_cov, 2,2,Qca[8] );
00483         
00484         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 0,0,Qca[0] );
00485         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 0,1,Qca[1] );
00486         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 0,2,Qca[2] );
00487         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 1,0,Qca[3] );
00488         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 1,1,Qca[4] );
00489         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 1,2,Qca[5] );
00490         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 2,0,Qca[6] );
00491         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 2,1,Qca[7] );
00492         cvSetReal2D(list.motion_models.ca_y_kalman->process_noise_cov, 2,2,Qca[8] );
00493         
00494         cvZero(list.motion_models.cv_x_kalman->state_post);
00495         cvZero(list.motion_models.cv_y_kalman->state_post);
00496         
00497         cvZero(list.motion_models.ca_x_kalman->state_post);
00498         cvZero(list.motion_models.ca_y_kalman->state_post);
00499         
00500         return;
00501 }
00502 
00507 void GetErrorConvariance(t_errors*error)
00508 {
00510         
00511         CvMat*xi_error_element = cvCreateMat(2,2,CV_64FC1);
00512         CvMat*xr_error_element = cvCreateMat(2,2,CV_64FC1);
00513         CvMat*yi_error_element = cvCreateMat(2,2,CV_64FC1);
00514         CvMat*yr_error_element = cvCreateMat(2,2,CV_64FC1);
00515         CvMat*lateral_error_element = cvCreateMat(2,2,CV_64FC1);
00516         
00517         CvMat*xi_element = cvCreateMat(2,1,CV_64FC1);
00518         CvMat*xr_element = cvCreateMat(2,1,CV_64FC1);
00519         CvMat*yi_element = cvCreateMat(2,1,CV_64FC1);
00520         CvMat*yr_element = cvCreateMat(2,1,CV_64FC1);
00521         CvMat*lateral_element = cvCreateMat(2,1,CV_64FC1);
00522         
00523         CvMat*xi_element_t = cvCreateMat(1,2,CV_64FC1);
00524         CvMat*xr_element_t = cvCreateMat(1,2,CV_64FC1);
00525         CvMat*yi_element_t = cvCreateMat(1,2,CV_64FC1);
00526         CvMat*yr_element_t = cvCreateMat(1,2,CV_64FC1);
00527         CvMat*lateral_element_t = cvCreateMat(1,2,CV_64FC1);
00528         
00529         CvMat*xi_error_accumulator = cvCreateMat(2,2,CV_64FC1);
00530         CvMat*xr_error_accumulator = cvCreateMat(2,2,CV_64FC1);
00531         CvMat*yi_error_accumulator = cvCreateMat(2,2,CV_64FC1);
00532         CvMat*yr_error_accumulator = cvCreateMat(2,2,CV_64FC1);
00533         CvMat*lateral_error_accumulator = cvCreateMat(2,2,CV_64FC1);
00534         
00535         CvMat*ones = cvCreateMat(2,2,CV_64FC1);
00536         
00537         double multiplier=1;
00538         
00540         
00541         cvSet(xi_error_accumulator,cvScalar(0),NULL);
00542         cvSet(xr_error_accumulator,cvScalar(0),NULL);
00543         cvSet(yi_error_accumulator,cvScalar(0),NULL);
00544         cvSet(yr_error_accumulator,cvScalar(0),NULL);
00545         cvSet(lateral_error_accumulator,cvScalar(0),NULL);
00546         
00548         for(unsigned int i=0;i<error->number_points;i++)
00549         {
00550                 //              printf("x_cv_innovation %2.2f\n",error->x_cv_innovation[i]);
00551                 cvmSet(xi_element,0,0,error->x_innovation[i]);
00552                 cvmSet(xi_element,1,0,0);
00553                 
00554                 cvmSet(xr_element,0,0,error->x_residue[i]);
00555                 cvmSet(xr_element,1,0,0);
00556                 
00557                 cvmSet(yi_element,0,0,error->y_innovation[i]);
00558                 cvmSet(yi_element,1,0,0);
00559                 
00560                 cvmSet(yr_element,0,0,error->y_residue[i]);
00561                 cvmSet(yr_element,1,0,0);
00562                 
00563                 cvmSet(lateral_element,0,0,error->lateral_error[i]);
00564                 cvmSet(lateral_element,1,0,0);
00565                 
00566                 cvTranspose(xi_element,xi_element_t);
00567                 cvTranspose(xr_element,xr_element_t);
00568                 cvTranspose(yi_element,yi_element_t);
00569                 cvTranspose(yr_element,yr_element_t);
00570                 cvTranspose(lateral_element,lateral_element_t);
00571                 
00572                 cvMatMul(xi_element,xi_element_t,xi_error_element);
00573                 cvMatMul(xr_element,xr_element_t,xr_error_element);
00574                 cvMatMul(yi_element,yi_element_t,yi_error_element);
00575                 cvMatMul(yr_element,yr_element_t,yr_error_element);
00576                 cvMatMul(lateral_element,lateral_element_t,lateral_error_element);
00577                 
00578                 cvAdd(xi_error_element,xi_error_accumulator,xi_error_accumulator,NULL);
00579                 cvAdd(xr_error_element,xr_error_accumulator,xr_error_accumulator,NULL);
00580                 cvAdd(yi_error_element,yi_error_accumulator,yi_error_accumulator,NULL);
00581                 cvAdd(yr_error_element,yr_error_accumulator,yr_error_accumulator,NULL);
00582                 cvAdd(lateral_error_element,lateral_error_accumulator,lateral_error_accumulator,NULL);
00583         }
00584         
00585         if(error->number_points==0)
00586                 goto gecEND;
00587         
00588         cvSet(ones,cvScalar(1));
00589         multiplier=1./(double)error->number_points;
00590         //printf("Muli %2.8f\n",multiplier);
00592         
00593         cvMul(xi_error_accumulator,ones,xi_error_accumulator,multiplier);
00594         cvMul(xr_error_accumulator,ones,xr_error_accumulator,multiplier);
00595         cvMul(yi_error_accumulator,ones,yi_error_accumulator,multiplier);
00596         cvMul(yr_error_accumulator,ones,yr_error_accumulator,multiplier);
00597         cvMul(lateral_error_accumulator,ones,lateral_error_accumulator,multiplier);
00598         
00599         error->x_inno_cov = cvGetReal2D(xi_error_accumulator,0,0);
00600         error->x_resi_cov = cvGetReal2D(xr_error_accumulator,0,0);
00601         error->y_inno_cov = cvGetReal2D(yi_error_accumulator,0,0);
00602         error->y_resi_cov = cvGetReal2D(yr_error_accumulator,0,0);
00603         error->lateral_error_cov = cvGetReal2D(lateral_error_accumulator,0,0);
00604         
00605         gecEND:
00606         
00607         cvReleaseMat(&(xi_error_element));
00608         cvReleaseMat(&(xr_error_element));
00609         cvReleaseMat(&(yi_error_element));
00610         cvReleaseMat(&(yr_error_element));
00611         cvReleaseMat(&(lateral_error_element));
00612         
00613         cvReleaseMat(&(xi_element));
00614         cvReleaseMat(&(xr_element));
00615         cvReleaseMat(&(yi_element));
00616         cvReleaseMat(&(yr_element));
00617         cvReleaseMat(&(lateral_element));
00618         
00619         cvReleaseMat(&(xi_element_t));
00620         cvReleaseMat(&(xr_element_t));
00621         cvReleaseMat(&(yi_element_t));
00622         cvReleaseMat(&(yr_element_t));
00623         cvReleaseMat(&(lateral_element_t));
00624         
00625         cvReleaseMat(&(xi_error_accumulator));
00626         cvReleaseMat(&(xr_error_accumulator));
00627         cvReleaseMat(&(yi_error_accumulator));
00628         cvReleaseMat(&(yr_error_accumulator));
00629         cvReleaseMat(&(lateral_error_accumulator));
00630         cvReleaseMat(&(ones));
00631         
00632         return;
00633 }
00634 
00635 
00636 
00637 CvKalman*CreateModelCTRV(void)
00638 {
00639         double vQ=1;
00640         double vR=10;
00641         double vP=5;
00642         
00643         CvKalman*model=cvCreateKalman(5,5,0);
00644         
00645         cvSetIdentity(model->measurement_matrix,cvRealScalar(1));
00646         cvSetReal2D(model->measurement_matrix,0,0,0);
00647         cvSetReal2D(model->measurement_matrix,1,0,0);
00648         //      cvSetReal2D(model->measurement_matrix,2,0,0);
00649         cvSetReal2D(model->measurement_matrix,4,0,0);
00650         
00651         cvSetIdentity(model->measurement_noise_cov,cvRealScalar(vR));
00652         //      cvSetReal2D(model->measurement_noise_cov,3,0,vR);
00653         //      cvSetReal2D(model->measurement_noise_cov,4,0,vR);
00654         
00655         cvSetIdentity(model->process_noise_cov,cvRealScalar(vQ));
00656         //      cvSetReal2D(model->process_noise_cov,3,0,vQ*0.001);
00657         //      cvSetReal2D(model->process_noise_cov,4,0,vQ);
00658         
00659         cvSet(model->state_post,cvRealScalar(0.01));
00660         cvSetIdentity(model->error_cov_post,cvRealScalar(vP));
00661         
00662         UpdateTransitionMatrixCTRV(model,0,0,0,0,0.01,1./50.);
00663         /*
00664         printf("A[0][0] %f\n",cvGetReal2D(model->transition_matrix,0,0));
00665         printf("A[0][1] %f\n",cvGetReal2D(model->transition_matrix,0,1));
00666         printf("A[0][2] %f\n",cvGetReal2D(model->transition_matrix,0,2));
00667         printf("A[0][3] %f\n",cvGetReal2D(model->transition_matrix,0,3));
00668         printf("A[0][4] %f\n",cvGetReal2D(model->transition_matrix,0,4));
00669         */
00670         return model;
00671 }
00672 
00673 CvKalman*CreateModelCV_SC(void)
00674 {
00675         double vR=180;
00676         double vP=100;
00677         
00678         double dt=1./50.,a=12.;
00679         
00680         CvKalman*model=cvCreateKalman(2,2,0);
00681         
00682         cvSet(model->state_post,cvRealScalar(0));
00683         
00684         cvSetReal2D(model->measurement_matrix,0,0,1);
00685         cvSetReal2D(model->measurement_matrix,1,0,0);
00686         
00687         cvSetIdentity(model->measurement_noise_cov,cvRealScalar(vR));
00688         
00689         cvSetIdentity(model->error_cov_post,cvRealScalar(vP));
00690         
00691         UpdateTransitionMatrixCV_SC(model,dt);
00692         
00693         cvSetReal2D(model->process_noise_cov,0,0,a*a*pow(dt,3)/3);
00694         cvSetReal2D(model->process_noise_cov,0,1,a*a*pow(dt,2)/2);
00695         cvSetReal2D(model->process_noise_cov,1,0,a*a*pow(dt,2)/2);
00696         cvSetReal2D(model->process_noise_cov,1,1,a*a*pow(dt,1));
00697         
00698         //      printf("Q:\n");
00699         //      printf("%6.2f %6.2f\n",cvGetReal2D(model->process_noise_cov,0,0),cvGetReal2D(model->process_noise_cov,0,1));
00700         //      printf("%6.2f %6.2f\n",cvGetReal2D(model->process_noise_cov,1,0),cvGetReal2D(model->process_noise_cov,1,1));
00701         
00702         return model;
00703 }
00704 
00705 CvKalman*CreateModelCV(void)
00706 {
00707         double vR=600;
00708         double vP=500;
00709         
00710         double dt=1./50.,a=10;
00711         
00712         CvKalman*model=cvCreateKalman(4,4,0);
00713         
00714         cvSet(model->state_post,cvRealScalar(0));
00715         
00716         cvSetReal2D(model->measurement_matrix,0,0,0);
00717         cvSetReal2D(model->measurement_matrix,1,0,1);
00718         cvSetReal2D(model->measurement_matrix,2,0,0);
00719         cvSetReal2D(model->measurement_matrix,3,0,1);
00720         
00721         cvSetIdentity(model->measurement_noise_cov,cvRealScalar(vR));
00722         
00723         cvSetIdentity(model->error_cov_post,cvRealScalar(vP));
00724         
00725         UpdateTransitionMatrixCV(model,dt);
00726         
00727         cvSetReal2D(model->process_noise_cov,0,0,a*a*pow(dt,3)/3);
00728         cvSetReal2D(model->process_noise_cov,0,1,a*a*pow(dt,2)/2);
00729         cvSetReal2D(model->process_noise_cov,0,2,0);
00730         cvSetReal2D(model->process_noise_cov,0,3,0);
00731         
00732         cvSetReal2D(model->process_noise_cov,1,0,a*a*pow(dt,2)/2);
00733         cvSetReal2D(model->process_noise_cov,1,1,a*a*pow(dt,1));
00734         cvSetReal2D(model->process_noise_cov,1,2,0);
00735         cvSetReal2D(model->process_noise_cov,1,3,0);
00736         
00737         cvSetReal2D(model->process_noise_cov,2,0,0);
00738         cvSetReal2D(model->process_noise_cov,2,1,0);
00739         cvSetReal2D(model->process_noise_cov,2,2,a*a*pow(dt,3)/3);
00740         cvSetReal2D(model->process_noise_cov,2,3,a*a*pow(dt,2)/2);
00741         
00742         cvSetReal2D(model->process_noise_cov,3,0,0);
00743         cvSetReal2D(model->process_noise_cov,3,1,0);
00744         cvSetReal2D(model->process_noise_cov,3,2,a*a*pow(dt,2)/2);
00745         cvSetReal2D(model->process_noise_cov,3,3,a*a*pow(dt,1));
00746         
00747         //      printf("Q:\n");
00748         //      printf("%6.2f %6.2f %6.2f %6.2f\n",cvGetReal2D(model->process_noise_cov,0,0),cvGetReal2D(model->process_noise_cov,0,1),cvGetReal2D(model->process_noise_cov,0,2),cvGetReal2D(model->process_noise_cov,0,3));
00749         //      printf("%6.2f %6.2f %6.2f %6.2f\n",cvGetReal2D(model->process_noise_cov,1,0),cvGetReal2D(model->process_noise_cov,1,1),cvGetReal2D(model->process_noise_cov,1,2),cvGetReal2D(model->process_noise_cov,1,3));
00750         //      printf("%6.2f %6.2f %6.2f %6.2f\n",cvGetReal2D(model->process_noise_cov,2,0),cvGetReal2D(model->process_noise_cov,2,1),cvGetReal2D(model->process_noise_cov,2,2),cvGetReal2D(model->process_noise_cov,2,3));
00751         //      printf("%6.2f %6.2f %6.2f %6.2f\n",cvGetReal2D(model->process_noise_cov,3,0),cvGetReal2D(model->process_noise_cov,3,1),cvGetReal2D(model->process_noise_cov,3,2),cvGetReal2D(model->process_noise_cov,3,3));
00752         
00753         return model;
00754 }
00755 
00756 CvKalman*CreateModelFwdCt(void)
00757 {
00758         double vR=500;
00759         double vP=100;
00760         double vQ=10;
00761         double dt=1./50.;
00762         
00763         CvKalman*model=cvCreateKalman(6,2,0);
00764         
00765         dH_FwdCt(model);
00766         
00767         cvSetIdentity(model->measurement_noise_cov,cvRealScalar(vR));
00768         
00769         cvSetIdentity(model->process_noise_cov,cvRealScalar(vQ));
00770         cvSetIdentity(model->error_cov_post,cvRealScalar(vP));
00771         
00772         double q0[]={0, 0, 0, 0, 0, 0};
00773         
00774         dA_FwdCt(model,q0,dt);
00775         
00776         return model;
00777 }
00778 
00779 void IterateMotionModelCTRV(CvKalman*model,double vm,double wm)
00780 {
00781         double x,y,t,v,w;
00782         static bool init=true;
00783         
00784         cvKalmanPredict(model);
00785         
00786         CvMat*measurement=cvCreateMat( 5, 1, CV_32FC1 );
00787         
00788         cvZero(measurement);
00789         cvSetReal2D(measurement,3,0,vm);
00790         cvSetReal2D(measurement,2,0,wm);
00791         
00792         //      printf("vM %f wM %f\n",vm,wm);
00793         
00794         x=cvGetReal2D(model->state_post,0,0);
00795         y=cvGetReal2D(model->state_post,1,0);
00796         t=cvGetReal2D(model->state_post,2,0);
00797         v=cvGetReal2D(model->state_post,3,0);
00798         w=cvGetReal2D(model->state_post,4,0);
00799         
00800         UpdateTransitionMatrixCTRV(model,x,y,t,v,w,1./50.);
00801         
00802         //      printf("iX %f\n",x);
00803         //      printf("iY %f\n",y);
00804         //      printf("iT %f\n",t);
00805         //      printf("iV %f\n",v);
00806         //      printf("iW %f\n",w);
00807         
00808         cvKalmanCorrect(model,measurement);
00809         
00810         if(init)
00811         {
00812                 cvSet(model->state_post,cvRealScalar(0.01));
00813                 init=false;
00814         }
00815         
00816         x=cvGetReal2D(model->state_post,0,0);
00817         y=cvGetReal2D(model->state_post,1,0);
00818         t=cvGetReal2D(model->state_post,2,0);
00819         v=cvGetReal2D(model->state_post,3,0);
00820         w=cvGetReal2D(model->state_post,4,0);
00821         
00822 //      vel=v;
00823 //      theta=t;
00824         
00825         //      printf("X %f\n",x);
00826         //      printf("Y %f\n",y);
00827         //      printf("T %f\n",t);
00828         //      printf("V %f\n",v);
00829         //      printf("W %f\n",w);
00830         
00831         
00832         
00833         //      cvSetReal2D(model->state_post,3,0,0.1);
00834         //      cvSetReal2D(model->state_pre,3,0,0.1);
00835         
00836         //      double cv_pestimated_y=cvGetReal2D(list->motion_models.cv_y_kalman->state_post,0,0);
00837         
00838 }
00839 
00840 
00841 void IterateMotionModelFwdCt(CvKalman*model,double z[2])
00842 {
00843         //      static vectorll runtime_PSM;
00844         static CvMat*measurement=cvCreateMat( 2, 1, CV_32FC1 );
00845         double q[6];
00846                 
00847         q[0]=cvGetReal2D(model->state_post,0,0);
00848         q[1]=cvGetReal2D(model->state_post,1,0);
00849         q[2]=cvGetReal2D(model->state_post,2,0);
00850         q[3]=cvGetReal2D(model->state_post,3,0);
00851         q[4]=cvGetReal2D(model->state_post,4,0);
00852         q[5]=cvGetReal2D(model->state_post,5,0);
00853         
00854         cvSetReal2D(measurement,0,0,z[0]);
00855         cvSetReal2D(measurement,1,0,z[1]);
00856         
00857         dA_FwdCt(model,q,1/50.);//update transition matrix
00858         cvKalmanCorrect(model,measurement);
00859         cvKalmanPredict(model);
00860         
00861         q[0]=cvGetReal2D(model->state_post,0,0);
00862         q[1]=cvGetReal2D(model->state_post,1,0);
00863         q[2]=cvGetReal2D(model->state_post,2,0);
00864         q[3]=cvGetReal2D(model->state_post,3,0);
00865         q[4]=cvGetReal2D(model->state_post,4,0);
00866         q[5]=cvGetReal2D(model->state_post,5,0);
00867         
00868 //      memcpy(s,q,6*sizeof(double));
00869         
00870         printf("X %6.6f Y %6.6f V1 %6.6f T %6.6f F %6.6f V2 %6.6f\n",q[0],q[1],q[2],q[3],q[4],q[5]);
00871 }
00872 
00873 double IterateMotionModelCV_SC(CvKalman*model,double vm)
00874 {
00875         double x;
00876         
00877         CvMat*measurement=cvCreateMat( 2, 1, CV_32FC1 );
00878         
00879         cvZero(measurement);
00880         cvSetReal2D(measurement,0,0,vm);
00881         
00882         //      printf("Vx %f\n",vxm);
00883         //      printf("z:\n %6.2f %6.2f\n",cvGetReal2D(measurement,0,0),cvGetReal2D(measurement,1,0));
00884         
00885         //      printf("H:\n %6.2f %6.2f\n",cvGetReal2D(model->measurement_matrix,0,0),cvGetReal2D(model->measurement_matrix,1,0));
00886         
00887         //      x=cvGetReal2D(model->state_post,0,0);
00888         //      vx=cvGetReal2D(model->state_post,1,0);
00889         
00890         //      printf("x:\n %6.2f %6.2f\n",x,vx);
00891         
00892         cvKalmanCorrect(model,measurement);
00893         
00894         x=cvGetReal2D(model->state_post,0,0);
00895         //      vx=cvGetReal2D(model->state_post,1,0);
00896         
00897         //      vel=sqrt(pow(vx,2)+pow(vy,2));
00898         //      theta=atan2(vy,vx);
00899         
00900         //      printf("x:\n %6.2f %6.2f\n",x,vx);
00901         
00902         cvKalmanPredict(model);
00903         
00904         //      double gx=cvGetReal2D(model->gain,0,0);
00905         //      double gvx=cvGetReal2D(model->gain,1,1);
00906         
00907         //      printf("G:\n %6.2f %6.2f\n",gx,gvx);
00908         
00909         return x;
00910 }
00911 
00912 void IterateMotionModelCV(CvKalman*model,double vxm,double vym)
00913 {
00914         CvMat*measurement=cvCreateMat( 4, 1, CV_32FC1 );
00915         
00916         cvZero(measurement);
00917         cvSetReal2D(measurement,1,0,vxm);
00918         cvSetReal2D(measurement,3,0,vym);
00919         
00920         //      printf("Vx %f\n",vxm);
00921         //      printf("z:\n %6.2f %6.2f %6.2f %6.2f\n",cvGetReal2D(measurement,0,0),cvGetReal2D(measurement,1,0),cvGetReal2D(measurement,2,0),cvGetReal2D(measurement,3,0));
00922         
00923         //      printf("H:\n %6.2f %6.2f %6.2f %6.2f\n",cvGetReal2D(model->measurement_matrix,0,0),cvGetReal2D(model->measurement_matrix,1,0),cvGetReal2D(model->measurement_matrix,2,0),cvGetReal2D(model->measurement_matrix,3,0));
00924         
00925 //      x=cvGetReal2D(model->state_post,0,0);
00926 //      vx=cvGetReal2D(model->state_post,1,0);
00927 //      y=cvGetReal2D(model->state_post,2,0);
00928 //      vy=cvGetReal2D(model->state_post,3,0);
00929         
00930         //      printf("x:\n %6.2f %6.2f %6.2f %6.2f\n",x,vx,y,vy);
00931         
00932         cvKalmanCorrect(model,measurement);
00933         
00934 //      x=cvGetReal2D(model->state_post,0,0);
00935 //      vx=cvGetReal2D(model->state_post,1,0);
00936 //      y=cvGetReal2D(model->state_post,2,0);
00937 //      vy=cvGetReal2D(model->state_post,3,0);
00938         
00939 //      vel=sqrt(pow(vx,2)+pow(vy,2));
00940 //      theta=atan2(vy,vx);
00941         
00942         //      printf("x:\n %6.2f %6.2f %6.2f %6.2f\n",x,vx,y,vy);
00943         
00944         cvKalmanPredict(model);
00945         
00946         //      double gx=cvGetReal2D(model->gain,0,0);
00947         //      double gvx=cvGetReal2D(model->gain,1,1);
00948         //      double gy=cvGetReal2D(model->gain,2,2);
00949         //      double gvy=cvGetReal2D(model->gain,3,3);
00950         
00951         //      printf("G:\n %6.2f %6.2f %6.2f %6.2f\n",gx,gvx,gy,gvy);
00952 }
00953 
00954 void UpdateTransitionMatrixCV_SC(CvKalman*model,double dt)
00955 {
00956         double tM[2][2];
00957         
00958         tM[0][0]=1;
00959         tM[0][1]=dt;
00960         
00961         tM[1][0]=0;
00962         tM[1][1]=1;
00963         
00964         cvSetReal2D(model->transition_matrix, 0,0,tM[0][0]);
00965         cvSetReal2D(model->transition_matrix, 0,1,tM[0][1]);
00966         cvSetReal2D(model->transition_matrix, 1,0,tM[1][0]);
00967         cvSetReal2D(model->transition_matrix, 1,1,tM[1][1]);
00968         
00969         //      printf("A:\n");
00970         //      printf("%6.2f %6.2f\n",tM[0][0],tM[0][1]);
00971         //      printf("%6.2f %6.2f\n",tM[1][0],tM[1][1]);
00972 }
00973 
00974 void UpdateTransitionMatrixCV(CvKalman*model,double dt)
00975 {
00976         double tM[4][4];
00977         
00978         tM[0][0]=1;
00979         tM[0][1]=dt;
00980         tM[0][2]=0;
00981         tM[0][3]=0;
00982         
00983         tM[1][0]=0;
00984         tM[1][1]=1;
00985         tM[1][2]=0;
00986         tM[1][3]=0;
00987         
00988         tM[2][0]=0;
00989         tM[2][1]=0;
00990         tM[2][2]=1;
00991         tM[2][3]=dt;
00992         
00993         tM[3][0]=0;
00994         tM[3][1]=0;
00995         tM[3][2]=0;
00996         tM[3][3]=1;
00997         
00998         cvSetReal2D(model->transition_matrix, 0,0,tM[0][0]);
00999         cvSetReal2D(model->transition_matrix, 0,1,tM[0][1]);
01000         cvSetReal2D(model->transition_matrix, 0,2,tM[0][2]);
01001         cvSetReal2D(model->transition_matrix, 0,3,tM[0][3]);
01002         
01003         cvSetReal2D(model->transition_matrix, 1,0,tM[1][0]);
01004         cvSetReal2D(model->transition_matrix, 1,1,tM[1][1]);
01005         cvSetReal2D(model->transition_matrix, 1,2,tM[1][2]);
01006         cvSetReal2D(model->transition_matrix, 1,3,tM[1][3]);
01007         
01008         cvSetReal2D(model->transition_matrix, 2,0,tM[2][0]);
01009         cvSetReal2D(model->transition_matrix, 2,1,tM[2][1]);
01010         cvSetReal2D(model->transition_matrix, 2,2,tM[2][2]);
01011         cvSetReal2D(model->transition_matrix, 2,3,tM[2][3]);
01012         
01013         cvSetReal2D(model->transition_matrix, 3,0,tM[3][0]);
01014         cvSetReal2D(model->transition_matrix, 3,1,tM[3][1]);
01015         cvSetReal2D(model->transition_matrix, 3,2,tM[3][2]);
01016         cvSetReal2D(model->transition_matrix, 3,3,tM[3][3]);
01017         
01018         //      printf("A:\n");
01019         //      printf("%6.2f %6.2f %6.2f %6.2f\n",tM[0][0],tM[0][1],tM[0][2],tM[0][3]);
01020         //      printf("%6.2f %6.2f %6.2f %6.2f\n",tM[1][0],tM[1][1],tM[1][2],tM[1][3]);
01021         //      printf("%6.2f %6.2f %6.2f %6.2f\n",tM[2][0],tM[2][1],tM[2][2],tM[2][3]);
01022         //      printf("%6.2f %6.2f %6.2f %6.2f\n",tM[3][0],tM[3][1],tM[3][2],tM[3][3]);
01023 }
01024 
01025 void dH_FwdCt(CvKalman*model)
01026 {
01027         cvZero(model->measurement_matrix);
01028         
01029         //      0     0     1     0     0         0
01030         //  0     0     0     0     1     0
01031         
01032         cvSetReal2D(model->measurement_matrix,0,2,1);
01033         cvSetReal2D(model->measurement_matrix,1,4,1);
01034         
01035 }
01036 void dA_FwdCt(CvKalman*model,double q[6],double dt,double l)
01037 {
01038         //Calculate dA in x[k-1]
01039         
01040         //      double x=q[0];
01041         //      double y=q[1];
01042         double v1=q[2];
01043         double t=q[3];
01044         double f=q[4];
01045         //      double v2=q[5];
01046         
01047         double tm[6][6];
01048         
01049         printf("\n>> V1 %f DT %f\n",v1,dt);
01050         printf("T %f\n",t);
01051         printf("F %f\n",f);
01052         printf("\n");
01053         
01054         //dF1
01055         tm[0][0]=1;
01056         tm[0][1]=0;
01057         tm[0][2]=cos(t)*cos(f)*dt;
01058         tm[0][3]=-sin(t)*cos(f)*v1*dt;
01059         tm[0][4]=cos(t)*(-sin(f))*v1*dt;
01060         tm[0][5]=0;
01061         
01062         tm[1][0]=0;
01063         tm[1][1]=1;
01064         tm[1][2]=sin(t)*cos(f)*dt;
01065         tm[1][3]=cos(t)*cos(f)*v1*dt;
01066         tm[1][4]=sin(t)*(-sin(f))*v1*dt;
01067         tm[1][5]=0;
01068         
01069         tm[2][0]=0;
01070         tm[2][1]=0;
01071         tm[2][2]=1;
01072         tm[2][3]=0;
01073         tm[2][4]=0;
01074         tm[2][5]=0;
01075         
01076         tm[3][0]=0;
01077         tm[3][1]=0;
01078         tm[3][2]=sin(f/l)*dt;
01079         tm[3][3]=1;
01080         tm[3][4]=cos(f/l)/l*v1*dt;
01081         tm[3][5]=0;
01082         
01083         tm[4][0]=0;
01084         tm[4][1]=0;
01085         tm[4][2]=0;
01086         tm[4][3]=0;
01087         tm[4][4]=1;
01088         tm[4][5]=dt;
01089         
01090         tm[5][0]=0;
01091         tm[5][1]=0;
01092         tm[5][2]=0;
01093         tm[5][3]=0;
01094         tm[5][4]=0;
01095         tm[5][5]=1;
01096         
01097         cvSetReal2D(model->transition_matrix, 0,0,tm[0][0]);
01098         cvSetReal2D(model->transition_matrix, 0,1,tm[0][1]);
01099         cvSetReal2D(model->transition_matrix, 0,2,tm[0][2]);
01100         cvSetReal2D(model->transition_matrix, 0,3,tm[0][3]);
01101         cvSetReal2D(model->transition_matrix, 0,4,tm[0][4]);
01102         cvSetReal2D(model->transition_matrix, 0,5,tm[0][5]);
01103         
01104         cvSetReal2D(model->transition_matrix, 1,0,tm[1][0]);
01105         cvSetReal2D(model->transition_matrix, 1,1,tm[1][1]);
01106         cvSetReal2D(model->transition_matrix, 1,2,tm[1][2]);
01107         cvSetReal2D(model->transition_matrix, 1,3,tm[1][3]);
01108         cvSetReal2D(model->transition_matrix, 1,4,tm[1][4]);
01109         cvSetReal2D(model->transition_matrix, 1,5,tm[1][5]);
01110         
01111         cvSetReal2D(model->transition_matrix, 2,0,tm[2][0]);
01112         cvSetReal2D(model->transition_matrix, 2,1,tm[2][1]);
01113         cvSetReal2D(model->transition_matrix, 2,2,tm[2][2]);
01114         cvSetReal2D(model->transition_matrix, 2,3,tm[2][3]);
01115         cvSetReal2D(model->transition_matrix, 2,4,tm[2][4]);
01116         cvSetReal2D(model->transition_matrix, 2,5,tm[2][5]);
01117         
01118         cvSetReal2D(model->transition_matrix, 3,0,tm[3][0]);
01119         cvSetReal2D(model->transition_matrix, 3,1,tm[3][1]);
01120         cvSetReal2D(model->transition_matrix, 3,2,tm[3][2]);
01121         cvSetReal2D(model->transition_matrix, 3,3,tm[3][3]);
01122         cvSetReal2D(model->transition_matrix, 3,4,tm[3][4]);
01123         cvSetReal2D(model->transition_matrix, 3,5,tm[3][5]);
01124         
01125         cvSetReal2D(model->transition_matrix, 4,0,tm[4][0]);
01126         cvSetReal2D(model->transition_matrix, 4,1,tm[4][1]);
01127         cvSetReal2D(model->transition_matrix, 4,2,tm[4][2]);
01128         cvSetReal2D(model->transition_matrix, 4,3,tm[4][3]);
01129         cvSetReal2D(model->transition_matrix, 4,4,tm[4][4]);
01130         cvSetReal2D(model->transition_matrix, 4,5,tm[4][5]);
01131         
01132         cvSetReal2D(model->transition_matrix, 5,0,tm[5][0]);
01133         cvSetReal2D(model->transition_matrix, 5,1,tm[5][1]);
01134         cvSetReal2D(model->transition_matrix, 5,2,tm[5][2]);
01135         cvSetReal2D(model->transition_matrix, 5,3,tm[5][3]);
01136         cvSetReal2D(model->transition_matrix, 5,4,tm[5][4]);
01137         cvSetReal2D(model->transition_matrix, 5,5,tm[5][5]);
01138         
01139         printf("dA:\n");
01140         printf("%6.6f %6.6f %6.6f %6.6f %6.6f %6.6f\n",tm[0][0],tm[0][1],tm[0][2],tm[0][3],tm[0][4],tm[0][5]);
01141         printf("%6.6f %6.6f %6.6f %6.6f %6.6f %6.6f\n",tm[1][0],tm[1][1],tm[1][2],tm[1][3],tm[1][4],tm[1][5]);
01142         printf("%6.6f %6.6f %6.6f %6.6f %6.6f %6.6f\n",tm[2][0],tm[2][1],tm[2][2],tm[2][3],tm[2][4],tm[2][5]);
01143         printf("%6.6f %6.6f %6.6f %6.6f %6.6f %6.6f\n",tm[3][0],tm[3][1],tm[3][2],tm[3][3],tm[3][4],tm[3][5]);
01144         printf("%6.6f %6.6f %6.6f %6.6f %6.6f %6.6f\n",tm[4][0],tm[4][1],tm[4][2],tm[4][3],tm[4][4],tm[4][5]);
01145         printf("%6.6f %6.6f %6.6f %6.6f %6.6f %6.6f\n",tm[5][0],tm[5][1],tm[5][2],tm[5][3],tm[5][4],tm[5][5]);
01146         
01147         // [                    1,                    0,     cos(t)*cos(f)*dt, -sin(t)*cos(f)*v1*dt, -cos(t)*sin(f)*v1*dt,                    0]
01148         // [                    0,                    1,     sin(t)*cos(f)*dt,  cos(t)*cos(f)*v1*dt, -sin(t)*sin(f)*v1*dt,                    0]
01149         // [                    0,                    0,                    1,                    0,                    0,                    0]
01150         // [                    0,                    0,          sin(f/l)*dt,                    1,     cos(f/l)/l*v1*dt,                    0]
01151         // [                    0,                    0,                    0,                    0,                    1,                   dt]
01152         // [                    0,                    0,                    0,                    0,                    0,                    1]
01153 }
01154 
01155 void UpdateTransitionMatrixCTRV(CvKalman*model,double /*x*/,double /*y*/,double t,double v,double w,double dt)
01156 {
01157         double tM[5][5];
01158         
01159         tM[0][0]=1;
01160         tM[0][1]=0;
01161         tM[0][2]=(v/w)*cos(w*dt+t)-(v/w)*cos(t);
01162         tM[0][3]=(1/w)*sin(w*dt+t)-(1/w)*sin(t);
01163         tM[0][4]=-(v/pow(w,2))*sin(w*dt+t)+(v/w)*cos(w*dt+t)*dt+(v/pow(w,2))*sin(t);
01164         
01165         tM[1][0]=0;
01166         tM[1][1]=1;
01167         tM[1][2]=(v/w)*sin(w*dt+t)+(v/w)*cos(t);
01168         tM[1][3]=-(1/w)*cos(w*dt+t)+(1/w)*sin(t);
01169         tM[1][4]=(v/pow(w,2))*cos(w*dt*t)+(v/w)*sin(w*dt+t)*dt-(v/pow(w,2))*sin(t);
01170         
01171         tM[2][0]=0;
01172         tM[2][1]=0;
01173         tM[2][2]=1;
01174         tM[2][3]=0;
01175         tM[2][4]=dt;
01176         //      printf("DT %f\n",dt);
01177         
01178         tM[3][0]=0;
01179         tM[3][1]=0;
01180         tM[3][2]=0;
01181         tM[3][3]=1;
01182         tM[3][4]=0;
01183         
01184         tM[4][0]=0;
01185         tM[4][1]=0;
01186         tM[4][2]=0;
01187         tM[4][3]=0;
01188         tM[4][4]=1;
01189         
01190         
01191         cvSetReal2D(model->transition_matrix, 0,0,tM[0][0]);
01192         cvSetReal2D(model->transition_matrix, 0,1,tM[0][1]);
01193         cvSetReal2D(model->transition_matrix, 0,2,tM[0][2]);
01194         cvSetReal2D(model->transition_matrix, 0,3,tM[0][3]);
01195         cvSetReal2D(model->transition_matrix, 0,4,tM[0][4]);
01196         
01197         cvSetReal2D(model->transition_matrix, 1,0,tM[1][0]);
01198         cvSetReal2D(model->transition_matrix, 1,1,tM[1][1]);
01199         cvSetReal2D(model->transition_matrix, 1,2,tM[1][2]);
01200         cvSetReal2D(model->transition_matrix, 1,3,tM[1][3]);
01201         cvSetReal2D(model->transition_matrix, 1,4,tM[1][4]);
01202         
01203         cvSetReal2D(model->transition_matrix, 2,0,tM[2][0]);
01204         cvSetReal2D(model->transition_matrix, 2,1,tM[2][1]);
01205         cvSetReal2D(model->transition_matrix, 2,2,tM[2][2]);
01206         cvSetReal2D(model->transition_matrix, 2,3,tM[2][3]);
01207         cvSetReal2D(model->transition_matrix, 2,4,tM[2][4]);
01208         
01209         cvSetReal2D(model->transition_matrix, 3,0,tM[3][0]);
01210         cvSetReal2D(model->transition_matrix, 3,1,tM[3][1]);
01211         cvSetReal2D(model->transition_matrix, 3,2,tM[3][2]);
01212         cvSetReal2D(model->transition_matrix, 3,3,tM[3][3]);
01213         cvSetReal2D(model->transition_matrix, 3,4,tM[3][4]);
01214         
01215         cvSetReal2D(model->transition_matrix, 4,0,tM[4][0]);
01216         cvSetReal2D(model->transition_matrix, 4,1,tM[4][1]);
01217         cvSetReal2D(model->transition_matrix, 4,2,tM[4][2]);
01218         cvSetReal2D(model->transition_matrix, 4,3,tM[4][3]);
01219         cvSetReal2D(model->transition_matrix, 4,4,tM[4][4]);
01220         
01221         //      printf("A:\n");
01222         //      printf("%6.2f %6.2f %6.2f %6.2f %6.2f\n",tM[0][0],tM[0][1],tM[0][2],tM[0][3],tM[0][4]);
01223         //      printf("%6.2f %6.2f %6.2f %6.2f %6.2f\n",tM[1][0],tM[1][1],tM[1][2],tM[1][3],tM[1][4]);
01224         //      printf("%6.2f %6.2f %6.2f %6.2f %6.2f\n",tM[2][0],tM[2][1],tM[2][2],tM[2][3],tM[2][4]);
01225         //      printf("%6.2f %6.2f %6.2f %6.2f %6.2f\n",tM[3][0],tM[3][1],tM[3][2],tM[3][3],tM[3][4]);
01226         //      printf("%6.2f %6.2f %6.2f %6.2f %6.2f\n",tM[4][0],tM[4][1],tM[4][2],tM[4][3],tM[4][4]);
01227 }
/home/laradmin/lar/perception/planarobstacles/mtt/src/mtt_kalman.cpp
