ExtendedKalmanFilter

代码说明：

  Extended Kalman Filter Tracking Object in 3-D(Assume that we want to track an object moving in 3-D space with constant velocity. Our instruments observe bearing, range and high(cylindrical coordinates). However, of an interest are rectangular coordinates. Since transformation is non-linear this requires use of extended Kalman filter. Because transformation is non-linear between X,Y and Range,Bearing and linear between Z and high(Z is height), this problems serves as a good comparason of how well extended Kalman filter can perform. By comparing its linear estimation error in Z to non-linear estimations in X and Y, we can judge how non-familiarities effect estimation. )