#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?0.0225;

??//measurement?covariance?matrix?-?radar
??R_radar_?<??????????????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_.x_?=?VectorXd（4）;
????ekf_.x_?<
????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?<??cout?<}

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????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-master\CMakeLists.txt
?????目錄???????????0??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\Docs\
?????文件????????1230??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\Docs\Data_Flow_Doc.txt
?????文件?????????417??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\Docs\Input_Output?File?Format.txt
?????文件????????1075??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\LICENSE
?????文件????????6008??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\README.md
?????文件????????1102??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\cmakepatch.txt
?????目錄???????????0??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\data\
?????文件???????66250??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\data\obj_pose-laser-radar-synthetic-input.txt
?????目錄???????????0??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\
?????目錄???????????0??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\Eclipse\
?????文件????????2974??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\Eclipse\README.md
?????目錄???????????0??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\Eclipse\images\
?????文件???????43835??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\Eclipse\images\Capture-EclipseMenuFileImport.jpg
?????文件???????38857??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\Eclipse\images\Capture-EclipseProjectsIntoWorkspace.jpg
?????文件??????179467??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\Eclipse\images\Capture-Final.png
?????文件???????69973??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\Eclipse\images\Capture-Import-2.png
?????文件?????????140??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\README.md
?????目錄???????????0??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\
?????文件????????3838??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\README.md
?????目錄???????????0??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\
?????文件??????357399??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\arguments_selection.png
?????文件??????350358??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\arguments_tab.png
?????文件??????546473??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\at_breakpoint.png
?????文件???????89586??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\breakpoint.png
?????文件???????46200??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\build_button.png
?????文件???????62774??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\edit_scheme.png
?????文件???????63216??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\scheme_selection.png
?????文件??????128915??2018-12-14?00:45??CarND-Extended-Kalman-Filter-Project-master\ide_profiles\xcode\images\schemes_location.png
............此處省略376個(gè)文件信息