/**
* Updates the state and the state covariance matrix using a laser measurement.
* @param {MeasurementPackage} meas_package
*/
void UKF::UpdateLidar(MeasurementPackage meas_package) {
VectorXd z = meas_package.raw_measurements_;
MatrixXd Zsig = MatrixXd(2, 2 * n_aug_ + 1);
VectorXd z_pred = VectorXd(2);
MatrixXd S = MatrixXd(2, 2);
//transform sigma points into measurement space
for (int i = 0; i < 2 * n_aug_ + 1; i++) {
double px = Xsig_pred_(0, i);
double py = Xsig_pred_(1, i);
Zsig(0, i) = px;
Zsig(1, i) = py;
}
// measurement noise covariance matrix
MatrixXd R = MatrixXd(2, 2);
R << std_laspx_ * std_laspx_, 0,
0, std_laspy_ * std_laspy_;
// Predict radar measurement
PredictMeasurement(2, Zsig, z_pred, S, R);
// Update state
UpdateState(z, z_pred, S, Zsig);
// Calculate NIS
NIS_laser_ = (z - z_pred).transpose() * S.inverse() * (z - z_pred);
}
VectorXd Tools::PolarToCartesian(const VectorXd& x_in) {
/**
* Convert polar coordinates to Cartesian:
* Input is a 3x vector in polar coordinate: rho, theta, rho_dot
* Output is 4x vector in Cartesian coordinates: px, py, vx, vy
*/
VectorXd x_out = VectorXd(4);
x_out.fill(0.0);
float rho = x_in(0);
float theta = x_in(1);
float rho_dot = x_in(2);
// normalize y
float pi_2 = 2*M_PI;
while(theta > M_PI) theta -= pi_2;
while(theta < -M_PI) theta += pi_2;
float px = rho * cos(theta);
float py = rho * sin(theta);
float vx = rho_dot * cos(theta);
float vy = rho_dot * sin(theta);
x_out << px, py, vx, vy;
return x_out;
}
void KalmanFilter::UpdateEKF(const VectorXd &z) {
/**
TODO:
* update the state by using Extended Kalman Filter equations
*/
float rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1));
float phi = atan2(x_(1), x_(0));
float rho_dot = (x_(0)*x_(2) + x_(1)*x_(3))/rho;
VectorXd h = VectorXd(3);
h << rho,phi,rho_dot;
VectorXd y = z - h;
y[1] = atan2(sin(y[1]),cos(y[1]));
MatrixXd Ht = H_.transpose();
MatrixXd S = H_ * P_ * Ht + R_;
MatrixXd Si = S.inverse();
MatrixXd PHt = P_ * Ht;
MatrixXd K = PHt * Si;
//new estimate
x_ = x_ + (K * y);
long x_size = x_.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}
/**
* Updates the state and the state covariance matrix using a radar measurement.
* @param {MeasurementPackage} meas_package
*/
void UKF::UpdateRadar(MeasurementPackage meas_package) {
VectorXd z = meas_package.raw_measurements_;
MatrixXd Zsig = MatrixXd(3, 2 * n_aug_ + 1);
VectorXd z_pred = VectorXd(3);
MatrixXd S = MatrixXd(3, 3);
//transform sigma points into radar measurement space
for (int i = 0; i < 2 * n_aug_ + 1; i++) {
double px = Xsig_pred_(0, i);
double py = Xsig_pred_(1, i);
double v = Xsig_pred_(2, i);
double yaw = Xsig_pred_(3, i);
double vx = cos(yaw) * v;
double vy = sin(yaw) * v;
Zsig(0, i) = sqrt(px * px + py * py); //rho
Zsig(1, i) = atan2(py, px); //phi
Zsig(2, i) = (px * vx + py * vy ) / sqrt(px * px + py * py); //rho_dot
}
//measurement noise covariance matrix
MatrixXd R = MatrixXd(3, 3);
R << std_radr_ * std_radr_, 0, 0,
0, std_radphi_ * std_radphi_, 0,
0, 0, std_radrd_ * std_radrd_;
// Predict radar measurement with given Sigma predictions
PredictMeasurement(3, Zsig, z_pred, S, R);
// Update the state
UpdateState(z, z_pred, S, Zsig);
// Calculate NIS
NIS_radar_ = (z - z_pred).transpose() * S.inverse() * (z - z_pred);
}
/**
* Initializes Unscented Kalman filter
*/
UKF::UKF() {
is_initialized_ = false;
// if this is false, laser measurements will be ignored (except during init)
use_laser_ = true;
// if this is false, radar measurements will be ignored (except during init)
use_radar_ = true;
// initial state vector
x_ = VectorXd(5);
// initial covariance matrix
P_ = MatrixXd(5, 5);
// Process noise standard deviation longitudinal acceleration in m/s^2
std_a_ = 1.5;
// Process noise standard deviation yaw acceleration in rad/s^2
std_yawdd_ = 0.57;
// Laser measurement noise standard deviation position1 in m
std_laspx_ = 0.15;
// Laser measurement noise standard deviation position2 in m
std_laspy_ = 0.15;
// Radar measurement noise standard deviation radius in m
std_radr_ = 0.3;
// Radar measurement noise standard deviation angle in rad
std_radphi_ = 0.03;
// Radar measurement noise standard deviation radius change in m/s
std_radrd_ = 0.3;
// State dimension
n_x_ = x_.size();
// Augmented state dimension
n_aug_ = n_x_ + 2; // We will create 2 * n_aug_ + 1 sigma points
// Number of sigma points
n_sig_ = 2 * n_aug_ + 1;
// Set the predicted sigma points matrix dimentions
Xsig_pred_ = MatrixXd(n_x_, n_sig_);
// Sigma point spreading parameter
lambda_ = 3 - n_aug_;
// Weights of sigma points
weights_ = VectorXd(n_sig_);
// Measurement noise covariance matrices initialization
R_radar_ = MatrixXd(3, 3);
R_radar_ << std_radr_*std_radr_, 0, 0,
0, std_radphi_*std_radphi_, 0,
0, 0,std_radrd_*std_radrd_;
R_lidar_ = MatrixXd(2, 2);
R_lidar_ << std_laspx_*std_laspx_,0,
0,std_laspy_*std_laspy_;
}
vector<VectorXd> PointsArea::points() const {
unsigned n = ps.size();
vector<VectorXd> vs(n, VectorXd(2));
for (unsigned i = 0; i < n; ++i) {
vs[i][0] = ps[i].x();
vs[i][1] = ps[i].y();
}
return vs;
}
VectorXd Tools::CartesianToPolar(const VectorXd& x_in) {
/**
* Convert Cartesian coordinates to polar:
* Input is 4x vector in Cartesian coordinates: px, py, vx, vy
* Output is a 3x vector in polar coordinate: rho, theta, rho_dot
*/
VectorXd x_out = VectorXd(3);
x_out.fill(0.0);
float rho;
float theta;
float rho_dot;
// Cartesian coordinates
float px = x_in(0),
py = x_in(1),
vx = x_in(2),
vy = x_in(3);
if (fabs(px) < 0.0001) {
cout << "Tools::CartesianToPolar() - Error - Devide by zero!" << endl;
px = 0.0001;
}
// calculate distance (range) rho
rho = sqrt(px*px + py*py);
if (fabs(rho) < 0.0001) {
cout << "Tools::CartesianToPolar() - Error - Devide by zero!" << endl;
rho = 0.0001;
}
// calculate angle theta in range -pi < theta < pi
theta = atan2(py, px);
// calculate range rate ro_dot
rho_dot = (px*vx + py*vy)/rho;
// output vector
x_out << rho, theta, rho_dot;
return x_out;
}
void UKF::AugmentSigmaPoints() {
//create augmented mean state
VectorXd x_aug_ = VectorXd(n_aug_);
x_aug_.fill(0);
x_aug_.head(5) = x_;
//create augmented covariance matrix
MatrixXd P_aug_ = MatrixXd(n_aug_, n_aug_);
P_aug_.fill(0.0);
P_aug_.topLeftCorner(n_x_, n_x_) = P_;
P_aug_(n_aug_ - 2, n_aug_ - 2) = std_a_ * std_a_;
P_aug_(n_aug_ - 1, n_aug_ - 1) = std_yawdd_ * std_yawdd_;
//create augmented sigma points
MatrixXd A = P_aug_.llt().matrixL();
Xsig_aug_.col(0) = x_aug_;
for (int i = 0; i < n_aug_; i++) {
Xsig_aug_.col(i + 1) = x_aug_ + sqrt(lambda_ + n_aug_) * A.col(i);
Xsig_aug_.col(i + 1 + n_aug_) = x_aug_ - sqrt(lambda_ + n_aug_) * A.col(i);
}
}
// Universal update function
void UKF::UpdateUKF(MeasurementPackage meas_package, MatrixXd Zsig, int n_z){
// Mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
z_pred = Zsig * weights_;
//measurement covariance matrix S
MatrixXd S = MatrixXd(n_z, n_z);
S.fill(0.0);
for (int i = 0; i < n_sig_; i++) {
// Residual
VectorXd z_diff = Zsig.col(i) - z_pred;
// Angle normalization
NormAng(&(z_diff(1)));
S = S + weights_(i) * z_diff * z_diff.transpose();
}
// Add measurement noise covariance matrix
MatrixXd R = MatrixXd(n_z, n_z);
if (meas_package.sensor_type_ == MeasurementPackage::RADAR){ // Radar
R = R_radar_;
}
else if (meas_package.sensor_type_ == MeasurementPackage::LASER){ // Lidar
R = R_lidar_;
}
S = S + R;
// Create matrix for cross correlation Tc
MatrixXd Tc = MatrixXd(n_x_, n_z);
// Calculate cross correlation matrix
Tc.fill(0.0);
for (int i = 0; i < n_sig_; i++) {
//residual
VectorXd z_diff = Zsig.col(i) - z_pred;
if (meas_package.sensor_type_ == MeasurementPackage::RADAR){ // Radar
// Angle normalization
NormAng(&(z_diff(1)));
}
// State difference
VectorXd x_diff = Xsig_pred_.col(i) - x_;
// Angle normalization
NormAng(&(x_diff(3)));
Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
}
// Measurements
VectorXd z = meas_package.raw_measurements_;
//Kalman gain K;
MatrixXd K = Tc * S.inverse();
// Residual
VectorXd z_diff = z - z_pred;
if (meas_package.sensor_type_ == MeasurementPackage::RADAR){ // Radar
// Angle normalization
NormAng(&(z_diff(1)));
}
// Update state mean and covariance matrix
x_ = x_ + K * z_diff;
P_ = P_ - K * S * K.transpose();
// Calculate NIS
if (meas_package.sensor_type_ == MeasurementPackage::RADAR){ // Radar
NIS_radar_ = z.transpose() * S.inverse() * z;
}
else if (meas_package.sensor_type_ == MeasurementPackage::LASER){ // Lidar
NIS_laser_ = z.transpose() * S.inverse() * z;
}
}
/**
* Predicts sigma points, the state, and the state covariance matrix.
* @param {double} delta_t the change in time (in seconds) between the last
* measurement and this one.
*/
void UKF::Prediction(double delta_t) {
double delta_t2 = delta_t*delta_t;
// Augmented mean vector
VectorXd x_aug = VectorXd(n_aug_);
// Augmented state covarience matrix
MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
// Sigma point matrix
MatrixXd Xsig_aug = MatrixXd(n_aug_, n_sig_);
// Fill the matrices
x_aug.fill(0.0);
x_aug.head(n_x_) = x_;
P_aug.fill(0);
P_aug.topLeftCorner(n_x_,n_x_) = P_;
P_aug(5,5) = std_a_*std_a_;
P_aug(6,6) = std_yawdd_*std_yawdd_;
// Square root of P matrix
MatrixXd L = P_aug.llt().matrixL();
// Create sigma points
Xsig_aug.col(0) = x_aug;
double sqrt_lambda_n_aug = sqrt(lambda_+n_aug_); // Save some computations
VectorXd sqrt_lambda_n_aug_L;
for(int i = 0; i < n_aug_; i++) {
sqrt_lambda_n_aug_L = sqrt_lambda_n_aug * L.col(i);
Xsig_aug.col(i+1) = x_aug + sqrt_lambda_n_aug_L;
Xsig_aug.col(i+1+n_aug_) = x_aug - sqrt_lambda_n_aug_L;
}
// Predict sigma points
for (int i = 0; i< n_sig_; i++)
{
// Extract values for better readability
double p_x = Xsig_aug(0,i);
double p_y = Xsig_aug(1,i);
double v = Xsig_aug(2,i);
double yaw = Xsig_aug(3,i);
double yawd = Xsig_aug(4,i);
double nu_a = Xsig_aug(5,i);
double nu_yawdd = Xsig_aug(6,i);
// Precalculate sin and cos for optimization
double sin_yaw = sin(yaw);
double cos_yaw = cos(yaw);
double arg = yaw + yawd*delta_t;
// Predicted state values
double px_p, py_p;
// Avoid division by zero
if (fabs(yawd) > EPS) {
double v_yawd = v/yawd;
px_p = p_x + v_yawd * (sin(arg) - sin_yaw);
py_p = p_y + v_yawd * (cos_yaw - cos(arg) );
}
else {
double v_delta_t = v*delta_t;
px_p = p_x + v_delta_t*cos_yaw;
py_p = p_y + v_delta_t*sin_yaw;
}
double v_p = v;
double yaw_p = arg;
double yawd_p = yawd;
// Add noise
px_p += 0.5*nu_a*delta_t2*cos_yaw;
py_p += 0.5*nu_a*delta_t2*sin_yaw;
v_p += nu_a*delta_t;
yaw_p += 0.5*nu_yawdd*delta_t2;
yawd_p += nu_yawdd*delta_t;
// Write predicted sigma point into right column
Xsig_pred_(0,i) = px_p;
Xsig_pred_(1,i) = py_p;
Xsig_pred_(2,i) = v_p;
Xsig_pred_(3,i) = yaw_p;
Xsig_pred_(4,i) = yawd_p;
}
// Predicted state mean
x_ = Xsig_pred_ * weights_; // vectorised sum
// Predicted state covariance matrix
P_.fill(0.0);
for (int i = 0; i < n_sig_; i++) { //iterate over sigma points
// State difference
VectorXd x_diff = Xsig_pred_.col(i) - x_;
// Angle normalization
NormAng(&(x_diff(3)));
P_ = P_ + weights_(i) * x_diff * x_diff.transpose() ;
}
}
void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
/*****************************************************************************
* Initialization
****************************************************************************/
if (!is_initialized_) {
/**
TODO:
* Initialize the state ekf_.x_ with the first measurement.
* Create the covariance matrix.
* Remember: you'll need to convert radar from polar to cartesian coordinates.
*/
// first measurement
ekf_.x_ = VectorXd(4);
ekf_.x_ << 1, 1, 1, 1;
if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
cout << "EKF : First measurement RADAR" << endl;
/**
Convert radar from polar to cartesian coordinates and initialize state.
*/
double rho = measurement_pack.raw_measurements_[0]; // range
double phi = measurement_pack.raw_measurements_[1]; // bearing
double rho_dot = measurement_pack.raw_measurements_[2]; // velocity of rho
// Coordinates convertion from polar to cartesian
double x = rho * cos(phi);
if (x < 0.0001) {
x = 0.0001;
}
double y = rho * sin(phi);
if (y < 0.0001) {
y = 0.0001;
}
double vx = rho_dot * cos(phi);
double vy = rho_dot * sin(phi);
ekf_.x_ << x, y, vx, vy;
}
else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
/**
Initialize state.
*/
// No velocity and coordinates are cartesian already.
cout << "EKF : First measurement LASER" << endl;
ekf_.x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
}
// Saving first timestamp in seconds
previous_timestamp_ = measurement_pack.timestamp_;
// done initializing, no need to predict or update
is_initialized_ = true;
return;
}
/*****************************************************************************
* Prediction
****************************************************************************/
/**
TODO:
* Update the state transition matrix F according to the new elapsed time.
- Time is measured in seconds.
* Update the process noise covariance matrix.
* Use noise_ax = 9 and noise_ay = 9 for your Q matrix.
*/
double dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
previous_timestamp_ = measurement_pack.timestamp_;
// State transition matrix update
ekf_.F_ = MatrixXd(4, 4);
ekf_.F_ << 1, 0, dt, 0,
0, 1, 0, dt,
0, 0, 1, 0,
0, 0, 0, 1;
// Noise covariance matrix computation
// Noise values from the task
double noise_ax = 9.0;
double noise_ay = 9.0;
double dt_2 = dt * dt; //dt^2
double dt_3 = dt_2 * dt; //dt^3
double dt_4 = dt_3 * dt; //dt^4
double dt_4_4 = dt_4 / 4; //dt^4/4
double dt_3_2 = dt_3 / 2; //dt^3/2
ekf_.Q_ = MatrixXd(4, 4);
ekf_.Q_ << dt_4_4 * noise_ax, 0, dt_3_2 * noise_ax, 0,
0, dt_4_4 * noise_ay, 0, dt_3_2 * noise_ay,
dt_3_2 * noise_ax, 0, dt_2 * noise_ax, 0,
0, dt_3_2 * noise_ay, 0, dt_2 * noise_ay;
ekf_.Predict();
/*****************************************************************************
* Update
****************************************************************************/
/**
TODO:
* Use the sensor type to perform the update step.
* Update the state and covariance matrices.
*/
if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
// Radar updates
ekf_.H_ = tools.CalculateJacobian(ekf_.x_);
ekf_.R_ = R_radar_;
ekf_.UpdateEKF(measurement_pack.raw_measurements_);
}
else {
// Laser updates
ekf_.H_ = H_laser_;
ekf_.R_ = R_laser_;
ekf_.Update(measurement_pack.raw_measurements_);
}
// print the output
//cout << "x_ = " << ekf_.x_ << endl;
//cout << "P_ = " << ekf_.P_ << endl;
}
/**
* Initializes Unscented Kalman filter
*/
UKF::UKF() {
initialized_ = false;
// if this is false, laser measurements will be ignored (except during init)
use_laser_ = true;
// if this is false, radar measurements will be ignored (except during init)
use_radar_ = true;
// in us
time_us_ = 0;
// Process noise standard deviation longitudinal acceleration in m/s^2
std_a_ = 0.8;
// Process noise standard deviation yaw acceleration in rad/s^2
std_yawdd_ = 0.55;
// Laser measurement noise standard deviation position1 in m
std_laspx_ = 0.15;
// Laser measurement noise standard deviation position2 in m
std_laspy_ = 0.15;
// Radar measurement noise standard deviation radius in m
std_radr_ = 0.3;
// Radar measurement noise standard deviation angle in rad
std_radphi_ = 0.03;
// Radar measurement noise standard deviation radius change in m/s
std_radrd_ = 0.3;
// state dimension (x, y, velocity, yaw_angle, yaw_rate)
n_x_ = 5;
// augmented state (n_x_ + 2)
n_aug_ = 7;
// hyper param for sigma points, 3 - n_aug_
lambda_ = -4;
// set weights
weights_ = VectorXd(2 * n_aug_ + 1);
weights_(0) = lambda_ / (lambda_ + n_aug_);
weights_.tail(2 * n_aug_).fill(0.5 / (lambda_ + n_aug_));
// initial state vector
x_ = VectorXd(n_x_);
x_.fill(0);
// initial covariance matrix
P_ = MatrixXd(n_x_, n_x_);
// matrix for sigma points
Xsig_aug_ = MatrixXd(n_aug_, 2 * n_aug_ + 1);
// matrix for predicted sigma points
Xsig_pred_ = MatrixXd(n_x_, 2 * n_aug_ + 1);
}
int main()
{
ofstream fout;
fout.open("./k-value.txt");
// 見出し
fout << "try";
for (int k = 0; k < K; k++) {
if (k < 100 || k % 5 == 0) {
fout << "\t" << k;
}
}
fout << endl;
// 係数
double λ, r1, r2;
double λ_max = 0.9, λ_min = 0.4;
//double c1 = 0.03, c2 = 0.03;
double c1 = 0.0001, c2 = 0.01;
int g_line = 1;
for (int m = 1; m <= M; m++) {
// Initial Points
vector<VectorXd> x(I);
// Initialize Matrix
vector<VectorXd> y(I);
vector<VectorXd> f(I);
vector<VectorXd> z(I);
vector<VectorXd> p(I);
vector<VectorXd> q(I);
vector<VectorXd> v(I);
vector<VectorXd> dx(I);
// Approximate matrix of the Hessian of the inverse matrix
vector<MatrixXd> J(I);
// Update matrix in the DFP
MatrixXd G = MatrixXd::Zero(N, N);
// The best point in the group
VectorXd x_gbest(N);
double f_gbest, f_temp;
for (int i = 0; i < I; i++) {
x[i] = VectorXd(N);
__initialPoints (&(x[i]), &g_line);
J[i] = MatrixXd::Identity(N, N);
f[i] = VectorXd::Zero(N);
v[i] = VectorXd::Zero(N);
dx[i] = VectorXd::Zero(N);
}
fout << m;
for (int k = 0; k < K; k++) {
z = f;
for (int i = 0; i < I; i++) {
// Differential function
for (int n = 0; n < N; n++) {
f[i](n) = 4 * pow(x[i](n), 3) - 32 * x[i](n) + 5;
}
// PSO Algorithm
f_temp = __2n_minima(&(x[i]));
if ((k == 0 && i == 0) || f_gbest > f_temp) {
x_gbest = x[i];
f_gbest = f_temp;
}
}
if (k < 100 || k % 5 == 0) {
fout << "\t" << f_gbest;
}
if (k != 0) {
// quasi-Newton Algorithm
for (int i = 0; i < I; i++) {
p[i] = v[i];
q[i] = f[i] - z[i];
// DFP 公式
G = (p[i] * p[i].transpose() / (p[i].transpose() * q[i])(0, 0)) -
(J[i] * q[i] * q[i].transpose() * J[i] / (q[i].transpose() * J[i] * q[i])(0, 0));
J[i] = J[i] + G;
dx[i] = J[i] * f[i];
}
}
// Inertia Weight Approach
λ = λ_max - (λ_max - λ_min) * k / K;
for (int i = 0; i < I; i++) {
r1 = ((double)rand() / ((double)RAND_MAX + 1)) * 1.0;
r2 = ((double)rand() / ((double)RAND_MAX + 1)) * 1.0;
v[i] = λ * v[i] - c1*r1*dx[i] + c2*r2*(x_gbest - x[i]);
x[i] = x[i] + v[i];
}
}
fout << endl;
}
