扩展卡尔曼滤波代码和数据

基于扩展卡尔曼滤波的车辆追踪项目，C++实现，CTRV模型，激光雷达和雷达传感器融合，详情见博客http://blog.csdn.net/adamshan/article/details/78359048

#include 
#include 
#include “ukf.h“
#include 
#include 
#include 
#include 
#include “ground_truth_package.h“
#include “Eigen/Dense“
#include “measurement_package.h“

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::vector;


void check_arguments（int argc char* argv[]） {
    string usage_instructions = “Usage instructions: “;
    usage_instructions += argv[0];
    usage_instructions += “ path/to/input.txt output.txt“;

    bool has_valid_args = false;

    // make sure the user has provided input and output files
    if （argc == 1） {
        cerr << usage_instructions << endl;
    } else if （argc == 2） {
        cerr << “Please include an output file.\n“ << usage_instructions << endl;
    } else if （argc == 3） {
        has_valid_args = true;
    } else if （argc > 3） {
        cerr << “Too many arguments.\n“ << usage_instructions << endl;
    }

    if （!has_valid_args） {
        exit（EXIT_FAILURE）;
    }
}

void check_files（ifstream& in_file string& in_name
                 ofstream& out_file string& out_name） {
    if （!in_file.is_open（）） {
        cerr << “Cannot open input file: “ << in_name << endl;
        exit（EXIT_FAILURE）;
    }

    if （!out_file.is_open（）） {
        cerr << “Cannot open output file: “ << out_name << endl;
        exit（EXIT_FAILURE）;
    }
}

VectorXd CalculateRMSE（const vector &estimations
                       const vector &ground_truth） {
    VectorXd rmse（4）;
    rmse << 0000;

    // 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（） == 0 || estimations.size（） != ground_truth.size（））{
        cout<<“the input is not legal!!!“<        return rmse;
    }

    //accumulate squared residuals
    for（int i=0; i < estimations.size（）; ++i）{
        VectorXd residual = estimations[i] - ground_truth[i];
        residual = residual.array（） * residual.array（）;
        rmse = rmse + residual;
    }

    //calculate the mean
    rmse = rmse / estimations.size（）;

    //calculate the squared root
    rmse = rmse.array（）.sqrt（）;

    //return the result
    return rmse;

}

int main（int argc char* argv[]） {
    check_arguments（argc argv）;

    string in_file_name_ = argv[1];
    ifstream in_file_（in_file_name_.c_str（） ifstream::in）;

    string out_file_name_ = argv[2];
    ofstream out_file_（out_file_name_.c_str（） ofstream::out）;

    check_files（in_file_ in_file_name_ out_file_ out_file_name_）;

    vector measurement_pack_list;
    vector gt_pack_list;

    string line;

    // prep the measurement packages （each line represents a measurement at a
    // timestamp）
    while （getline（in_file_ line）） {

        string sensor_type;
        MeasurementPackage meas_package;
        GroundTruthPackage gt_package;
    

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-10-26 13:27  ukf-code\
     文件        3136  2017-10-26 13:10  ukf-code\ukf.h
     文件       10929  2017-10-26 13:13  ukf-code\ukf.cpp
     文件         279  2017-09-06 11:32  ukf-code\measurement_package.h
     文件        7010  2017-10-26 13:12  ukf-code\main.cpp
     文件         341  2017-10-26 12:48  ukf-code\ground_truth_package.h
     文件         199  2017-10-26 13:10  ukf-code\CMakeLists.txt
     目录           0  2017-10-26 12:43  ukf-code\Eigen\
     文件        1594  2017-09-06 11:32  ukf-code\Eigen\IterativeLinearSolvers
     文件         637  2017-09-06 11:32  ukf-code\Eigen\QtAlignedMalloc
     文件        2189  2017-09-06 11:32  ukf-code\Eigen\OrderingMethods
     文件        1151  2017-09-06 11:32  ukf-code\Eigen\UmfPackSupport
     文件        1904  2017-09-06 11:32  ukf-code\Eigen\SuperLUSupport
     文件        1433  2017-09-06 11:32  ukf-code\Eigen\SparseCholesky
     文件         864  2017-09-06 11:32  ukf-code\Eigen\PardisoSupport
     文件        1670  2017-09-06 11:32  ukf-code\Eigen\CholmodSupport
     文件         607  2017-09-06 11:32  ukf-code\Eigen\CMakeLists.txt
     文件        1467  2017-09-06 11:32  ukf-code\Eigen\PaStiXSupport
     文件        3295  2017-09-06 11:32  ukf-code\Eigen\Eigen2Support
     文件         697  2017-09-06 11:32  ukf-code\Eigen\MetisSupport
     文件         712  2017-09-06 11:32  ukf-code\Eigen\LeastSquares
     文件         930  2017-09-06 11:32  ukf-code\Eigen\SPQRSupport
     文件         580  2017-09-06 11:32  ukf-code\Eigen\Householder
     文件        1394  2017-09-06 11:32  ukf-code\Eigen\Eigenvalues
     文件        1835  2017-09-06 11:32  ukf-code\Eigen\SparseCore
     文件         755  2017-09-06 11:32  ukf-code\Eigen\StdVector
     文件         749  2017-09-06 11:32  ukf-code\Eigen\StdDeque
     文件         991  2017-09-06 11:32  ukf-code\Eigen\SparseQR
     文件        1776  2017-09-06 11:32  ukf-code\Eigen\SparseLU
     文件        1605  2017-09-06 11:32  ukf-code\Eigen\Geometry
     文件         775  2017-09-06 11:32  ukf-code\Eigen\Cholesky
............此处省略342个文件信息