扩展卡尔曼滤波项目C++代码

#include “FusionEKF.h“
#include 
#include “Eigen/Dense“
#include “tools.h“

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::vector;

/**
 * Constructor.
 */
FusionEKF::FusionEKF（） {
  is_initialized_ = false;

  previous_timestamp_ = 0;

  // initializing matrices
  R_laser_ = MatrixXd（2 2）;
  R_radar_ = MatrixXd（3 3）;
  H_laser_ = MatrixXd（2 4）;
  Hj_ = MatrixXd（3 4）;

  //measurement covariance matrix - laser
  R_laser_ << 0.0225 0
              0 0.0225;

  //measurement covariance matrix - radar
  R_radar_ << 0.09 0 0
              0 0.0009 0
              0 0 0.09;

  /**
   * TODO: Finish initializing the FusionEKF.
   * TODO: Set the process and measurement noises
   */


}

/**
 * Destructor.
 */
FusionEKF::~FusionEKF（） {}

void FusionEKF::ProcessMeasurement（const MeasurementPackage &measurement_pack） {
  /**
   * Initialization
   */
  if （!is_initialized_） {
    /**
     * TODO: Initialize the state ekf_.x_ with the first measurement.
     * TODO: Create the covariance matrix.
     * You‘ll need to convert radar from polar to cartesian coordinates.
     */

    // first measurement
    cout << “EKF: “ << endl;
    ekf_.x_ = VectorXd（4）;
    ekf_.x_ << 1 1 1 1;

    if （measurement_pack.sensor_type_ == MeasurementPackage::RADAR） {
      // TODO: Convert radar from polar to cartesian coordinates 
      //         and initialize state.

    }
    else if （measurement_pack.sensor_type_ == MeasurementPackage::LASER） {
      // TODO: Initialize state.

    }

    // 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.
   * TODO: Update the process noise covariance matrix.
   * Use noise_ax = 9 and noise_ay = 9 for your Q matrix.
   */

  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） {
    // TODO: Radar updates

  } else {
    // TODO: Laser updates

  }

  // print the output
  cout << “x_ = “ << ekf_.x_ << endl;
  cout << “P_ = “ << ekf_.P_ << endl;
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-master
     文件         320  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-master.gitignore
     文件         676  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterCMakeLists.txt
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterDocs
     文件        1230  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterDocsData_Flow_Doc.txt
     文件         417  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterDocsInput_Output File Format.txt
     文件        1075  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterLICENSE
     文件        6008  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterREADME.md
     文件        1102  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-mastercmakepatch.txt
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterdata
     文件       66250  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masterdataobj_pose-laser-radar-synthetic-input.txt
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profiles
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesEclipse
     文件        2974  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesEclipseREADME.md
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesEclipseimages
     文件       43835  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesEclipseimagesCapture-EclipseMenuFileImport.jpg
     文件       38857  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesEclipseimagesCapture-EclipseProjectsIntoWorkspace.jpg
     文件      179467  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesEclipseimagesCapture-Final.png
     文件       69973  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesEclipseimagesCapture-Import-2.png
     文件         140  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesREADME.md
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcode
     文件        3838  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeREADME.md
     目录           0  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimages
     文件      357399  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesarguments_selection.png
     文件      350358  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesarguments_tab.png
     文件      546473  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesat_breakpoint.png
     文件       89586  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesreakpoint.png
     文件       46200  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesuild_button.png
     文件       62774  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesedit_scheme.png
     文件       63216  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesscheme_selection.png
     文件      128915  2018-12-14 00:45  CarND-Extended-Kalman-Filter-Project-masteride_profilesxcodeimagesschemes_location.png
............此处省略376个文件信息