// [[Rcpp::export]]
List myFastLm(MapMatd X, MapMatd y){
const int n(X.rows()), p(X.cols());
const LLT<MatrixXd> llt(AtA(X));
const VectorXd betahat(llt.solve(X.adjoint() * y));
const VectorXd fitted(X * betahat);
const VectorXd resid(y - fitted);
const int df(n - p);
const double s(resid.norm() / std::sqrt(double(df)));
const MatrixXd cov(llt.matrixL().solve(MatrixXd::Identity(p, p)));
const MatrixXd cov2(MatrixXd(p,p).setZero().selfadjointView<Lower>().rankUpdate(cov.adjoint()) );
return List::create(
Named("coefficients") = betahat,
Named("fitted.values") = fitted,
Named("residuals") = resid,
Named("s") = s,
Named("df.residual") = df,
Named("rank") = p,
Named("cov") = cov2*s*s);
}
RcppExport SEXP gls_direct(SEXP X, SEXP S, SEXP Y, SEXP maxit, SEXP tol)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Rcpp::List;
typedef Map<VectorXd> MapVecd;
typedef Map<Eigen::MatrixXd> MapMatd;
const int maxiter(as<int>(maxit));
const double toler(as<double>(tol));
const Eigen::Map<MatrixXd> XX(as<MapMatd>(X));
const Eigen::Map<MatrixXd> SS(as<MapMatd>(S));
const Eigen::Map<VectorXd> YY(as<MapVecd>(Y));
const int n(XX.rows());
const int p(XX.cols());
MatrixXd SX(MatrixXd(n, p));
SX = SS.llt().solve(XX);
MatrixXd XSX((XX.adjoint()) * SX);
VectorXd XSY((SX.adjoint()) * YY);
VectorXd beta = XSX.ldlt().solve(XSY);
return List::create(Named("beta") = beta);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
/**
* Updates the state and the state covariance matrix using a radar measurement.
* @param {MeasurementPackage} meas_package
*/
void UKF::UpdateRadar(MeasurementPackage meas_package) {
// Set measurement dimension, radar can measure r, phi, and r_dot
int n_z = 3;
// Create matrix for sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, n_sig_);
// Transform sigma points into measurement space
for (int i = 0; i < n_sig_; i++) {
// extract values for better readibility
double p_x = Xsig_pred_(0,i);
double p_y = Xsig_pred_(1,i);
double v = Xsig_pred_(2,i);
double yaw = Xsig_pred_(3,i);
double v1 = cos(yaw)*v;
double v2 = sin(yaw)*v;
// Measurement model
Zsig(0,i) = sqrt(p_x*p_x + p_y*p_y); //r
Zsig(1,i) = atan2(p_y,p_x); //phi
Zsig(2,i) = (p_x*v1 + p_y*v2 ) / Zsig(0,i); //r_dot
}
UpdateUKF(meas_package, Zsig, n_z);
}
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);
}
}
RcppExport SEXP matcopytest(SEXP X)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::Lower;
typedef float Scalar;
typedef double Double;
typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRXd;
typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Matrix> MapMat;
typedef Eigen::Map<const Vector> MapVec;
//const Eigen::Map<MatrixXd> A(as<Map<MatrixXd> >(X));
Rcpp::NumericMatrix xx(X);
const int n = xx.rows();
const int p = xx.cols();
MatrixRXd XX(n, p);
// Copy data
std::copy(xx.begin(), xx.end(), XX.data());
MatrixXd AtA(MatrixXd(p, p).setZero().selfadjointView<Lower>().rankUpdate(XX.adjoint()));
return wrap(AtA);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
//port faster cross product
RcppExport SEXP crossprodxval(SEXP X, SEXP idxvec_)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXi;
using Eigen::Lower;
using Rcpp::List;
typedef float Scalar;
typedef double Double;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRXd;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<Matrix> MapMat;
typedef Eigen::Map<MatrixRXd> MapRMat;
typedef Eigen::Map<const Vector> MapVec;
typedef Map<VectorXd> MapVecd;
typedef Map<VectorXi> MapVeci;
typedef Eigen::SparseVector<double> SparseVector;
typedef Eigen::SparseVector<int> SparseVectori;
const MapMat AA(as<MapMat>(X));
const MapVeci idxvec(as<MapVeci>(idxvec_));
const int n(AA.cols());
MatrixXd AtA(MatrixXd(n, n));
MatrixRXd A = AA;
int nfolds = idxvec.maxCoeff();
List xtxlist(nfolds);
for (int k = 1; k < nfolds + 1; ++k)
{
VectorXi idxbool = (idxvec.array() == k).cast<int>();
int nrow = idxbool.size();
int numelem = idxbool.sum();
VectorXi idx(numelem);
int c = 0;
for (int i = 0; i < nrow; ++i)
{
if (idxbool(i) == 1)
{
idx(c) = i;
++c;
}
}
int nvars = A.cols() - 1;
MatrixXd sub(numelem, n);
for (int r = 0; r < numelem; ++r)
{
sub.row(r) = A.row(idx(r));
}
MatrixXd AtAtmp(MatrixXd(n, n).setZero().
selfadjointView<Lower>().rankUpdate(sub.adjoint() ));
xtxlist[k-1] = AtAtmp;
}
return wrap(xtxlist);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
int main(int argc, char *argv[])
{
//
// command line parsing.
//
const char* inFile = parseStringParam("-in", argc, argv);
if (inFile == nullptr) printHelpExit();
const char* outFile = parseStringParam("-out", argc, argv);
if (outFile == nullptr) printHelpExit();
unsigned int outFacesN;
if (!parseIntParam("-outfaces", argc, argv, outFacesN)) printHelpExit();
unsigned int upsampleN;
if (!parseIntParam("-upsample", argc, argv, upsampleN)) printHelpExit();
// original mesh vertices and indices. This is the original mesh, which has a hole.
MatrixXd originalV;
MatrixXi originalF;
if (!igl::readOFF(inFile, originalV, originalF)) {
printHelpExit();
}
VectorXi originalLoop; // indices of the boundary of the hole.
igl::boundary_loop(originalF, originalLoop);
if (originalLoop.size() == 0) {
printf("Mesh has no hole!");
printHelpExit();
}
// upsample the original mesh. this makes fusing the original mesh with the patch much easier.
igl::upsample(Eigen::MatrixXd(originalV), Eigen::MatrixXi(originalF), originalV, originalF, upsampleN);
// compute boundary center.
VectorXd bcenter(3);
{
VectorXi R = originalLoop;
VectorXi C(3); C << 0, 1, 2;
MatrixXd B;
MatrixXd A = originalV;
igl::slice(A, R, C, B);
bcenter = (1.0f / originalLoop.size()) * B.colwise().sum();
}
// a flat patch that fills the hole.
MatrixXd patchV = MatrixXd(originalLoop.size() + 1, 3); // patch will have an extra vertex for the center vertex.
MatrixXi patchF = MatrixXi(originalLoop.size(), 3);
{
VectorXi R = originalLoop;
VectorXi C(3); C << 0, 1, 2;
MatrixXd A = originalV;
MatrixXd temp1;
igl::slice(A, R, C, temp1);
MatrixXd temp2(1, 3);
temp2 << bcenter(0), bcenter(1), bcenter(2);
// patch vertices will be the boundary vertices, plus a center vertex. concat these together.
igl::cat(1, temp1, temp2, patchV);
// create triangles that connect boundary vertices and the center vertex.
for (int i = 0; i < originalLoop.size(); ++i) {
patchF(i, 2) = (int)originalLoop.size();
patchF(i, 1) = i;
patchF(i, 0) = (1 + i) % originalLoop.size();
}
// also upsample patch. patch and original mesh will have the same upsampling level now
// making it trivial to fuse them together.
igl::upsample(Eigen::MatrixXd(patchV), Eigen::MatrixXi(patchF), patchV, patchF, upsampleN);
}
// the final mesh, where the patch and original mesh has been fused together.
std::vector<std::vector<double>> fusedV;
std::vector<std::vector<int>> fusedF;
int index = 0; // vertex index counter for the fused mesh.
// first, we add the upsampled patch to the fused mesh.
{
for (; index < patchV.rows(); ++index) {
fusedV.push_back({ patchV(index, 0), patchV(index, 1), patchV(index, 2) });
}
int findex = 0;
for (; findex < patchF.rows(); ++findex) {
fusedF.push_back({ patchF(findex, 0), patchF(findex, 1), patchF(findex, 2) });
}
}
// now, we fuse the patch and the original mesh together.
{
// remaps indices from the original mesh to the fused mesh.
std::map<int, int> originalToFusedMap;
for (int itri = 0; itri < originalF.rows(); ++itri) {
int triIndices[3];
for (int iv = 0; iv < 3; ++iv) {
int triIndex = originalF(itri, iv);
int ret;
if (originalToFusedMap.count(triIndex) == 0) {
int foundMatch = -1;
// the vertices at the boundary are the same, for both the two meshes(patch and original mesh).
// we ensure that these vertices are not duplicated.
// this is also how we ensure that the two meshes are fused together.
for (int jj = 0; jj < patchV.rows(); ++jj) {
VectorXd u(3); u << fusedV[jj][0], fusedV[jj][1], fusedV[jj][2];
VectorXd v(3); v << originalV(triIndex, 0), originalV(triIndex, 1), originalV(triIndex, 2);
if ((u - v).norm() < 0.00001) {
foundMatch = jj;
break;
}
}
if (foundMatch != -1) {
originalToFusedMap[triIndex] = foundMatch;
ret = foundMatch;
}
else {
fusedV.push_back({ originalV(triIndex, 0), originalV(triIndex, 1), originalV(triIndex, 2) });
originalToFusedMap[triIndex] = index;
ret = index;
index++;
}
}
else {
ret = originalToFusedMap[triIndex];
}
triIndices[iv] = ret;
}
fusedF.push_back({
triIndices[0],
triIndices[1],
triIndices[2] });
}
}
MatrixXd fairedV(fusedV.size(), 3);
MatrixXi fairedF(fusedF.size(), 3);
// now we shall do surface fairing on the mesh, to ensure
// that the patch conforms to the surrounding curvature.
{
for (int vindex = 0; vindex < fusedV.size(); ++vindex) {
auto r = fusedV[vindex];
fairedV(vindex, 0) = r[0];
fairedV(vindex, 1) = r[1];
fairedV(vindex, 2) = r[2];
}
for (int findex = 0; findex < fusedF.size(); ++findex) {
auto r = fusedF[findex];
fairedF(findex, 0) = r[0];
fairedF(findex, 1) = r[1];
fairedF(findex, 2) = r[2];
}
VectorXi b(fairedV.rows() - patchV.rows());
MatrixXd bc(fairedV.rows() - patchV.rows(), 3);
// setup the boundary conditions. This is simply the vertex positions of the vertices not part of the patch.
for (int i = (int)patchV.rows(); i < (int)fairedV.rows(); ++i) {
int jj = i - (int)patchV.rows();
b(jj) = i;
bc(jj, 0) = fairedV(i, 0);
bc(jj, 1) = fairedV(i, 1);
bc(jj, 2) = fairedV(i, 2);
}
MatrixXd Z;
int k = 2;
// surface fairing simply means that we solve the equation
// Delta^2 f = 0
// with appropriate boundary conditions.
// this function igl::harmonic from libigl takes care of that.
// note that this is pretty inefficient thought.
// the only boundary conditions necessary are the 2-ring of vertices around the patch.
// the rest of the mesh vertices need not be specified.
// we specify the rest of the mesh for simplicity of code, but it is not strictly necessary,
// and pretty inefficient, since we have to solve a LARGE matrix equation as a result of this.
igl::harmonic(fairedV, fairedF, b, bc, k, Z);
fairedV = Z;
}
// finally, we do a decimation step.
MatrixXd finalV(fusedV.size(), 3);
MatrixXi finalF(fusedF.size(), 3);
VectorXi temp0; VectorXi temp1;
igl::decimate(fairedV, fairedF, outFacesN, finalV, finalF, temp0, temp1);
igl::writeOFF(outFile, finalV, finalF);
}
MatrixXd AtA(const MapMatd& A) {
int n(A.cols());
return MatrixXd(n,n).setZero().selfadjointView<Lower>()
.rankUpdate(A.adjoint());
}
// 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() ;
}
}
// block conjugate gradient least squares
RcppExport SEXP block_cgls(SEXP A, SEXP b, SEXP lambda, SEXP maxit, SEXP tol, SEXP init)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Rcpp::List;
typedef Map<VectorXd> MapVecd;
typedef Map<Eigen::MatrixXd> MapMatd;
const int maxiter(as<int>(maxit));
const double toler(as<double>(tol));
const double lam(as<double>(lambda));
const Eigen::Map<MatrixXd> AA(as<MapMatd>(A));
const Eigen::Map<MatrixXd> bb(as<MapMatd>(b));
const Eigen::Map<MatrixXd> xinit(as<MapMatd>(init));
const int n(AA.cols());
const int m(AA.rows());
const int pp(bb.cols());
MatrixXd x(MatrixXd(n, pp));
MatrixXd r(MatrixXd(m, pp));
MatrixXd p(MatrixXd(n, pp));
MatrixXd s(MatrixXd(n, pp));
MatrixXd q(MatrixXd(m, pp));
x = xinit;
double norms0;
double norms;
MatrixXd gamma(MatrixXd(pp, pp));
MatrixXd gammaold(MatrixXd(pp, pp));
MatrixXd delta(MatrixXd(pp, pp));
double normx;
MatrixXd alpha(MatrixXd(pp, pp));
int iters = maxiter;
r = bb - AA * x;
s = AA.transpose() * r - (lam * x.array()).matrix();
p = s;
gamma = xtx(s);
norms0 = sqrt(gamma.diagonal().sum());
normx = x.norm();
for (int i = 0; i < maxiter; i++) {
q = AA * p;
delta = xtx(q) + (lam * xtx(p).array()).matrix();
alpha = delta.colPivHouseholderQr().solve(gamma);
x = x + p * alpha;
r = r - q * alpha;
s = AA.transpose() * r - (lam * x.array()).matrix();
gammaold = gamma;
norms = sqrt(gamma.diagonal().sum());
gamma = xtx(s);
normx = x.norm();
if (norms < norms0 * toler || normx * toler >= 1) {
iters = i;
break;
}
p = s + p * gammaold.colPivHouseholderQr().solve(gamma);
}
return List::create(Named("x") = x,
Named("iters") = iters);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
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);
}
