| 456 | |
| 457 | |
| 458 | void UKF::Interaction() { |
| 459 | |
| 460 | MatrixXd x_pre_cv = x_cv_; |
| 461 | MatrixXd x_pre_ctrv = x_ctrv_; |
| 462 | MatrixXd x_pre_rm = x_rm_; |
| 463 | MatrixXd P_pre_cv = P_cv_; |
| 464 | MatrixXd P_pre_ctrv = P_ctrv_; |
| 465 | MatrixXd P_pre_rm = P_rm_; |
| 466 | x_cv_ = modeMatchProbCV2CV_ *x_pre_cv + modeMatchProbCTRV2CV_ *x_pre_ctrv + modeMatchProbRM2CV_ *x_pre_rm; |
| 467 | x_ctrv_ = modeMatchProbCV2CTRV_*x_pre_cv + modeMatchProbCTRV2CTRV_*x_pre_ctrv + modeMatchProbRM2CTRV_*x_pre_rm; |
| 468 | x_rm_ = modeMatchProbCV2RM_ *x_pre_cv + modeMatchProbCTRV2RM_ *x_pre_ctrv + modeMatchProbRM2RM_*x_pre_rm; |
| 469 | |
| 470 | // not interacting yaw(-pi ~ pi) |
| 471 | x_cv_(3) = x_pre_cv(3); |
| 472 | x_ctrv_(3) = x_pre_ctrv(3); |
| 473 | x_rm_(3) = x_pre_rm(3); |
| 474 | |
| 475 | // cout<< "cv x state before interaction: " <<endl<<x_pre_cv<<endl; |
| 476 | // cout<< "ctrv x state before interaction: "<<endl<<x_pre_ctrv<<endl; |
| 477 | // cout<< "rm x state before interaction: " <<endl<<x_pre_rm<<endl<<endl; |
| 478 | // cout<< "rm match prob: "<< endl << modeMatchProbCV2RM_ << " "<<modeMatchProbCTRV2RM_<<" "<<modeMatchProbRM2RM_<<endl<<endl; |
| 479 | // cout<< "cv x state after interaction: " <<endl<<x_cv_<<endl; |
| 480 | // cout<< "ctrv x state after interaction: "<<endl<<x_ctrv_<<endl; |
| 481 | // cout<< "rm x state after interaction: " <<endl<<x_rm_<<endl<<endl; |
| 482 | // normalizing angle |
| 483 | while (x_cv_(3) > M_PI) x_cv_(3) -= 2.*M_PI; |
| 484 | while (x_cv_(3) <-M_PI) x_cv_(3) += 2.*M_PI; |
| 485 | while (x_ctrv_(3)> M_PI) x_ctrv_(3) -= 2.*M_PI; |
| 486 | while (x_ctrv_(3)<-M_PI) x_ctrv_(3) += 2.*M_PI; |
| 487 | while (x_rm_(3) > M_PI) x_rm_(3) -= 2.*M_PI; |
| 488 | while (x_rm_(3) <-M_PI) x_rm_(3) += 2.*M_PI; |
| 489 | |
| 490 | P_cv_ = modeMatchProbCV2CV_ *(P_pre_cv +(x_pre_cv - x_cv_)* (x_pre_cv - x_cv_).transpose()) + |
| 491 | modeMatchProbCTRV2CV_ *(P_pre_ctrv+(x_pre_ctrv -x_cv_)* (x_pre_ctrv -x_cv_).transpose())+ |
| 492 | modeMatchProbRM2CV_ *(P_pre_rm + (x_pre_rm - x_cv_)* (x_pre_rm - x_cv_).transpose()); |
| 493 | P_ctrv_ = modeMatchProbCV2CTRV_ *(P_pre_cv+ (x_pre_cv - x_ctrv_)* (x_pre_cv- x_ctrv_).transpose()) + |
| 494 | modeMatchProbCTRV2CTRV_*(P_pre_ctrv+(x_pre_ctrv -x_ctrv_)* (x_pre_ctrv -x_ctrv_).transpose())+ |
| 495 | modeMatchProbRM2CTRV_ *(P_pre_rm + (x_pre_rm - x_ctrv_)* (x_pre_rm - x_ctrv_).transpose()); |
| 496 | P_rm_ = modeMatchProbCV2RM_ *(P_pre_cv+ (x_pre_cv - x_rm_)* (x_pre_cv - x_rm_).transpose()) + |
| 497 | modeMatchProbCTRV2RM_ *(P_pre_ctrv+(x_pre_ctrv -x_rm_)* (x_pre_ctrv -x_rm_).transpose())+ |
| 498 | modeMatchProbRM2RM_ *(P_pre_rm + (x_pre_rm - x_rm_)* (x_pre_rm - x_rm_).transpose()); |
| 499 | |
| 500 | } |
| 501 | |
| 502 | |
| 503 | /** |