Skip to content

Caught the rogue robot using Unscented Kalman filter

July 12, 2017

Some time back, I wrote my experience of this exercise as part of the AI for Robotics course of Udacity. The final hunt problem.

I have some good news now. As part of the the Self driving car Engineer course, Udacity taught us about Unscented Kalman filters. Using which I was able to do the same problem, in a much more better way.

Shown below is a screen grab of such run. (Now it reliably takes around 6 seconds to catch the rogue car, and also in a very cool and optimal way)
success_caught_the_robot

My C++ code which complements the Unscented Kalman filter – the circle geometry part is shown below:
The header file:

#ifndef CIRCLE_GEOM_H#ifndef CIRCLE_GEOM_H#define CIRCLE_GEOM_H
#include <iostream>
#include "Eigen/Dense"
using namespace std;
using Eigen::MatrixXd;using Eigen::VectorXd;
class CircleGeom {
public: 
 double radius_; // center double cx_; double cy_;
 // dynamic data double x_; double y_; double dir_; double turn_rate_; double speed_;

CircleGeom(VectorXd& xm); virtual ~CircleGeom();
 /**   Dear Circle,     kindly tell me, which point in future should I target,    - Given my max speed is same as yours   - Thy now know youreself   - I'll tell you where I am right now   - also please tell me in what time I can expect to meet the rogue car I'm hunting 
   NOTE; The returned value is time and turn and distance are filled in, in passed arguments */ double tell_me_circle(double hunter_x, double hunter_y, double *turn_ret, double *dist_ret);
private:  double check_position(double hunter_x, double hunter_y, double tx, double ty, double *turn_ret, double *dist_ret);};
#endif /* CIRCLE_GEOM_H */

The cpp file:

#include "circle_geom.h"

#include "circle_geom.h"
double normalize_angle(double theta);
CircleGeom::CircleGeom(VectorXd& xm) { x_ = xm[0]; y_ = xm[1]; speed_ = xm[2]; dir_  = xm[3]; turn_rate_ = xm[4];

radius_ = fabs(speed_ / turn_rate_);
 if (turn_rate_ > 0) { cx_ = x_ + radius_*cos(M_PI/2 + dir_); cy_ = y_ + radius_*sin(M_PI/2 + dir_); } else { cx_ = x_ + radius_*cos(dir_ - M_PI/2); cy_ = y_ + radius_*sin(dir_ - M_PI/2); }
}
CircleGeom::~CircleGeom() {}
/**We use the logic of dividing the circle into pi/2 i.e 4 positions where we could go. We find out which positions are feasible and go to the closest.**/double CircleGeom::tell_me_circle(double hunter_x, double hunter_y, double *turn_ret, double *dist_ret){
 double ret = -1;
 for (double i =0; i<=2*M_PI; i+=M_PI/2.) { double xx = cx_ + radius_ * cos(i); double yy = cy_ + radius_ * sin(i); double turn_ret1; double dist_ret1; double tret = check_position(hunter_x, hunter_y, xx, yy, &turn_ret1, &dist_ret1); if (tret>0) { if (ret == -1) { *turn_ret = turn_ret1; *dist_ret = dist_ret1; ret = tret; } else { if (tret < ret) { *turn_ret = turn_ret1; *dist_ret = dist_ret1; ret = tret; } } } }

return ret;}
double CircleGeom::check_position(double hunter_x, double hunter_y, double tx, double ty, double *turn_ret, double *dist_ret) { double x_diff = tx - hunter_x; double y_diff = ty - hunter_y; double dist = sqrt(x_diff*x_diff+ y_diff* y_diff);
 // time taken by hunter car to reach the distance double hunter_t = dist / speed_;
 // time taken by rogue car to reach that  double theta1 = atan2(y_ - cy_, x_ - cx_); double theta2 = atan2(ty - cy_, tx - cx_); double dtheta = normalize_angle(theta2 - theta1);
 double dist_along_circle = dtheta*radius_; double rogue_t = dist_along_circle / speed_;
 if (hunter_t <rogue_t) { *dist_ret = dist; *turn_ret = atan2(ty - hunter_y, tx - hunter_x); return hunter_t; } else { return -1; }}
double normalize_angle(double theta) { //angle normalization    while (theta> M_PI) theta-=2.*M_PI;    while (theta<-M_PI) theta+=2.*M_PI;    return theta;}

 

Advertisements

From → Uncategorized

Leave a Comment

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: