CarND(term2): Extended Kalman Filter

Posted on |

sensor measurement

Radar

radio detection andranging, using radio waves to measure the distance to objects as well as their velocity and angle. Radar used a lot in preventing collions, parking assistance, cruise control. and Radar isn’t affected by weather conditions.

while Radar can’t tell an object’s shape correctly. and Radar can’t detect objects if they are out of their line of sight.

the Radar measurement data in EKF proejcct is 3D position and velocity vector (ro, theta, ro_dot) in polar coordinates.

Lidar

light detection and ranging, using near-infrared light to scan objects and create 3D map of the enviroment. it’s 360-degree view and can track movements and their directions. but it also depends on weather conditions and can’t detecting the speed of other vehicles well.

the Lidar measurement data in EKF proeject is 2D position vector (x,y) in Cartesian coordinate system.

compare Radar vs Lidar

server - client network

Udacity simulator communicate with EKF controller through websocket.

in simulators(Udacity carsim, Carla) running time, the message channel between simulator server and external controller need to be open all the time. so the simulator feed the controller with sensor data, and the controller feedback simulator with controlling data.

uwebSocket

built once uwebSocket, webSocket protocol providing full-duplex communication channel between server and client through a singlt TCP connection. it allows the server to send content ot the client without being first requested by the client, and allowing messages to be passed back and forth while keeping the conenction open.

sensor raw data

every data slice includes a either Lidar or Radar raw measurement and a ground truth measurement.

The state variable x is described in Cartesian coord, so for Radar measurement processing, there is a coordinate transfer from Cartesian to polar, and which lead it nonlinear, requiring Extended Karman Filter.

EKF controller

the client side is the EKF controller, which process the sensor measurement.

define 

system state `x_` ,

  state priori covariance `P_`,

   state transition matrix `F`, 

process covariance `Q_`, 

measurement gain matrix  'H'

measurement covariance 'R_' 

kalman filter gain 'K'

as discussed in previous blog: 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
 
void KalmanFilter::Predict()
{
	x_ = F * x_ ;   
	P_ = F * P_ * F.transpose() + Q_ ;
}
void KalmanFilter::Update(const Vector & measurement)
{
	if(EKF)
		h = toPolar(x_); 
		y = measurement - h ;
	else
		y = measurement - H * x_ ;
		 
	K = P_ * Ht / (H * P_ * Ht + R_);  
	x_ = x_ + K * y ; 
	P_ = (I - K*H) * P_ ;
 }