| 571 | } |
| 572 | |
| 573 | void UKF::Cv(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd, |
| 574 | double delta_t, vector<double>&state) { |
| 575 | //predicted state values |
| 576 | double px_p = p_x + v*cos(yaw)*delta_t; |
| 577 | double py_p = p_y + v*sin(yaw)*delta_t; |
| 578 | |
| 579 | double v_p = v; |
| 580 | // not sure which one, works better in curve by using yaw |
| 581 | double yaw_p = yaw; |
| 582 | // double yaw_p = 0; |
| 583 | double yawd_p = yawd; |
| 584 | |
| 585 | //add noise |
| 586 | px_p = px_p + 0.5 * nu_a * delta_t * delta_t * cos(yaw); |
| 587 | py_p = py_p + 0.5 * nu_a * delta_t * delta_t * sin(yaw); |
| 588 | v_p = v_p + nu_a*delta_t; |
| 589 | |
| 590 | yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t; |
| 591 | yawd_p = yawd_p + nu_yawdd*delta_t; |
| 592 | |
| 593 | state[0] = px_p; |
| 594 | state[1] = py_p; |
| 595 | state[2] = v_p; |
| 596 | state[3] = yaw_p; |
| 597 | state[4] = yawd_p; |
| 598 | |
| 599 | } |
| 600 | |
| 601 | |
| 602 | void UKF::randomMotion(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd, |