text: #include <iostream>
  #include "tools.h"
  
  
  
  using Eigen::VectorXd;
  using Eigen::MatrixXd;
  using std::vector;
  using std::max;
  
  Tools::Tools() {}
  
  Tools::~Tools() {}
  
  VectorXd Tools::CalculateRMSE(const vector<VectorXd> &estimations,
                                const vector<VectorXd> &ground_truth) {
  
  	VectorXd rmse(4);
  	rmse << 0, 0, 0, 0;
  	/* Reuse Lecture part*/
  	// check the validity of the following inputs:
  	//  * the estimation vector size should not be zero
  	//  * the estimation vector size should equal ground truth vector size
  	if (estimations.size() != ground_truth.size()
  		|| estimations.size() == 0) {
  		cout << "Invalid estimation or ground_truth data" << endl;
  		return rmse;
  	}
  
  	//accumulate squared residuals
  	for (unsigned int i = 0; i < estimations.size(); ++i) {
  
  		VectorXd residual = estimations[i] - ground_truth[i];
  
  		//coefficient-wise multiplication
  		residual = residual.array()*residual.array();
  		rmse += residual;
  	}
  
  	//calculate the mean
  	rmse = rmse / estimations.size();
  
  	//calculate the squared root
  	rmse = rmse.array().sqrt();
  
  	cout << "RMSE Done: " << rmse <<endl;
  	//return the result
  	return rmse;
  }
  
  MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) {
  	/* Reuse Lecture part*/
  	MatrixXd Hj(3, 4);
  	//recover state parameters
  	float px = x_state(0);
  	float py = x_state(1);
  	float vx = x_state(2);
  	float vy = x_state(3);
  
  	//pre-compute a set of terms to avoid repeated calculation
  	float c1 =  px * px + py * py;
  	float c2 = sqrt(c1);
  	float c3 = (c1*c2);
  
  	float zero_max = 1.0e-6;
  
  	//check division by zero
  	c1 = c1 < zero_max ? zero_max : c1;
  	c2 = c2 < zero_max ? zero_max : c2;
  	c3 = c3 < zero_max ? zero_max : c3;
  
  	//compute the Jacobian matrix
  	Hj << (px / c2), (py / c2), 0, 0,
  		-(py / c1), (px / c1), 0, 0,
  		py*(vx*py - vy * px) / c3, px*(px*vy - py * vx) / c3, px / c2, py / c2;
  
  	return Hj;
  }
  