├── readme_img
    ├── plot1.png
    └── plot2.png
├── include
    └── kalman_filter
    │   ├── ground_truth_package.h
    │   ├── measurement_package.h
    │   ├── tools.h
    │   ├── fusion_ekf.h
    │   ├── kalman_filter.h
    │   └── fusion_ukf.h
├── Docs
    ├── Input_Output File Format.txt
    └── Data_Flow_Doc.txt
├── CMakeLists.txt
├── README.md
├── src
    ├── kalman_filter.cpp
    ├── tools.cpp
    ├── fusion_ekf.cpp
    ├── main_ekf.cpp
    ├── main_ukf.cpp
    └── fusion_ukf.cpp
├── task.md
├── package.xml
├── results
    ├── output2.txt
    └── input2.log
└── data
    └── sample-laser-radar-measurement-data-2.txt


/readme_img/plot1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/JokerJohn/kalman_filter/HEAD/readme_img/plot1.png


--------------------------------------------------------------------------------
/readme_img/plot2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/JokerJohn/kalman_filter/HEAD/readme_img/plot2.png


--------------------------------------------------------------------------------
/include/kalman_filter/ground_truth_package.h:
--------------------------------------------------------------------------------
 1 | #ifndef GROUND_TRUTH_PACKAGE_H_
 2 | #define GROUND_TRUTH_PACKAGE_H_
 3 | 
 4 | #include "Eigen/Dense"
 5 | 
 6 | class GroundTruthPackage {
 7 | public:
 8 |   long timestamp_;
 9 | 
10 |   enum SensorType{
11 |     LASER,
12 |     RADAR
13 |   } sensor_type_;
14 | 
15 |   Eigen::VectorXd gt_values_;
16 | 
17 | };
18 | 
19 | #endif /* GROUND_TRUTH_PACKAGE_H_ */
20 | 


--------------------------------------------------------------------------------
/include/kalman_filter/measurement_package.h:
--------------------------------------------------------------------------------
 1 | #ifndef MEASUREMENT_PACKAGE_H_
 2 | #define MEASUREMENT_PACKAGE_H_
 3 | 
 4 | #include "Eigen/Dense"
 5 | 
 6 | class MeasurementPackage {
 7 | public:
 8 |   long long timestamp_;
 9 | 
10 |   enum SensorType{
11 |     LASER,
12 |     RADAR
13 |   } sensor_type_;
14 | 
15 |   Eigen::VectorXd raw_measurements_;
16 | };
17 | 
18 | #endif /* MEASUREMENT_PACKAGE_H_ */
19 | 


--------------------------------------------------------------------------------
/Docs/Input_Output File Format.txt:
--------------------------------------------------------------------------------
 1 | #L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
 2 | #R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
 3 | -----------------------------
 4 | Example
 5 | -----------------------------
 6 | R	8.60363	0.0290616	-2.99903	1477010443399637	8.6	0.25	-3.00029	0
 7 | L	8.45	0.25	1477010443349642	8.45	0.25	-3.00027	0 
 8 | 
 9 | #Output file format:
10 | est_px est_py est_vx est_vy meas_px meas_py gt_px gt_py gt_vx gt_vy


--------------------------------------------------------------------------------
/include/kalman_filter/tools.h:
--------------------------------------------------------------------------------
 1 | #ifndef TOOLS_H_
 2 | #define TOOLS_H_
 3 | #include <vector>
 4 | #include "Eigen/Dense"
 5 | 
 6 | class Tools {
 7 | public:
 8 |   /**
 9 |   * Constructor.
10 |   */
11 |   Tools();
12 | 
13 |   /**
14 |   * Destructor.
15 |   */
16 |   virtual ~Tools();
17 | 
18 |   /**
19 |   * A helper method to calculate RMSE.
20 |   */
21 |   Eigen::VectorXd CalculateRMSE(const std::vector<Eigen::VectorXd> &estimations, const std::vector<Eigen::VectorXd> &ground_truth);
22 | 
23 |   /**
24 |   * A helper method to calculate Jacobians.
25 |   */
26 |   Eigen::MatrixXd CalculateJacobian(const Eigen::VectorXd& x_state);
27 | 
28 | };
29 | 
30 | #endif /* TOOLS_H_ */
31 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(kalman_filter)
 3 | 
 4 | add_compile_options(-std=c++11)
 5 | find_package(Eigen3 QUIET)
 6 | find_package(catkin REQUIRED COMPONENTS
 7 |         roscpp
 8 |         rospy
 9 |         sensor_msgs
10 |         std_msgs
11 |         )
12 | catkin_package()
13 | 
14 | include_directories(
15 |         include
16 |         ${catkin_INCLUDE_DIRS}
17 |         ${Eigen3_INCLUDE_DIRS}
18 | )
19 | 
20 | set(SOURCE
21 |         src/fusion_ekf.cpp
22 |         src/kalman_filter.cpp
23 |         src/main_ekf.cpp
24 |         src/tools.cpp)
25 | add_executable(ekf_node ${SOURCE})
26 | target_link_libraries(ekf_node
27 |         ${catkin_LIBRARIES}
28 |         )
29 | 
30 | add_executable(ukf_node src/fusion_ukf.cpp src/main_ukf.cpp src/tools.cpp)
31 | target_link_libraries(ukf_node
32 |         ${catkin_LIBRARIES}
33 |         )


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | ## Udacity KF UKF EKF Project
 2 | 
 3 | Udacity毫米波，激光数据融合项目。
 4 | 
 5 | ## 数据
 6 | 
 7 | 激光测量：【vx, vy】
 8 | 
 9 | 毫米波测量：【ro, rhi,r_dot】
10 | 
11 | 当前数据可能是激光，也可能是毫米波，穿插着来。
12 | 
13 | 毫米波观测空间->状态空间变换是非线性的，可以采用EKF将h(x)近似线性化，经F矩阵转换到当前预测，线性转换后为高斯。也可以通过取上一帧观测sigma点，分别通过h(x)计算转换后的sigma，用这些sigma点来近似拟合一个高斯分布。
14 | 
15 | 激光雷达观测->状态空间是线性变换，可以直接写出来F矩阵。
16 | 
17 | ## KF/EKF
18 | 
19 | 预测当前位置和速度，状态变量【px, py, vx, vy】
20 | 
21 | 流程：
22 | 
23 | - 第一帧数据初始化EKF，包括状态量x_k-1，不确定性P_k-1
24 | - 后续每帧数据，计算F矩阵和过程噪声Q（，预测当前状态x_k, 不确定性P_k(状态空间)
25 | - 预测结果转换到观测空间（H矩阵）
26 | - 当前观测z和不确定度R,求残差（z-H*x_k）,残差不确定度（H*Q*H(T)+R）,卡尔曼增益K
27 | - 计算融合后的状态量x'和不确定度P'
28 | 
29 | ## KF/UKF
30 | 
31 | 预测当前位置、速度、偏航角及变化率，状态变量【px, py, v, yaw, yaw_rate】
32 | 
33 | - 第一帧数据初始化EKF，包括状态量x_k-1，不确定性P_k-1，sigma点权重
34 | - 根据h(x)计算sigma点转换后的点集矩阵（维度*点集）
35 | - 预测系统状态（sigma矩阵*权重），预测不确定度P(P_K-1+权重*维度变化量)
36 | - 计算拟合的高斯参数，根据观测计算残差、残差不确定度
37 | - 计算融合后的状态量x'和不确定度P'
38 | 
39 | ## 使用
40 | 
41 | 修改ekf_main.cpp和ukf_main.cpp中输入文件地址和输出文件地址，丢在catkin下编译即可。
42 | 
43 | ```c++
44 | rosrun kalman_filter  ekf_node
45 | rosrun kalman_filter  ukf_node
46 | ```
47 | 
48 | 


--------------------------------------------------------------------------------
/include/kalman_filter/fusion_ekf.h:
--------------------------------------------------------------------------------
 1 | #ifndef FusionEKF_H_
 2 | #define FusionEKF_H_
 3 | 
 4 | #include "measurement_package.h"
 5 | #include "Eigen/Dense"
 6 | #include <vector>
 7 | #include <string>
 8 | #include <fstream>
 9 | #include "kalman_filter.h"
10 | #include "tools.h"
11 | 
12 | class FusionEKF {
13 |  public:
14 |   /**
15 |   * Constructor.
16 |   */
17 |   FusionEKF();
18 | 
19 |   /**
20 |   * Destructor.
21 |   */
22 |   virtual ~FusionEKF();
23 | 
24 |   /**
25 |   * Run the whole flow of the Kalman Filter from here.
26 |   */
27 |   void ProcessMeasurement(const MeasurementPackage &measurement_pack);
28 | 
29 |   /**
30 |   * Kalman Filter update and prediction math lives in here.
31 |   */
32 |   KalmanFilter ekf_;
33 | 
34 |  private:
35 |   // check whether the tracking toolbox was initiallized or not (first measurement)
36 |   bool is_initialized_;
37 | 
38 |   // previous timestamp
39 |   long previous_timestamp_;
40 | 
41 |   // tool object used to compute Jacobian and RMSE
42 |   Tools tools;
43 |   Eigen::MatrixXd R_laser_;
44 |   Eigen::MatrixXd R_radar_;
45 |   Eigen::MatrixXd H_laser_;
46 |   Eigen::MatrixXd Hj_;
47 | };
48 | 
49 | #endif /* FusionEKF_H_ */
50 | 


--------------------------------------------------------------------------------
/Docs/Data_Flow_Doc.txt:
--------------------------------------------------------------------------------
 1 | Data Flow:
 2 | 1) The measuremennt processor/matlab simulator is generating the FUSION .txt file:
 3 | 	"data/obj_pose-laser-radar-synthetic-ukf-input.txt";
 4 | 	OR
 5 | 	"../matlab_examples/obj_pose-laser-radar-synthetic-ukf-input.txt";
 6 | 
 7 | The Input file format is:
 8 | #L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
 9 | #R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
10 | 
11 | Example:
12 | R	8.60363	0.0290616	-2.99903	1477010443399637	8.6	0.25	-3.00029	0
13 | L	8.45	0.25	1477010443349642	8.45	0.25	-3.00027	0
14 | 	
15 | 2) The EKF Algorithm reads form file reads all the lines and generates measurement structures
16 | 3) The MeasurementProcessor() is called with individual measurements (one by one). The results are saved
17 | (Attention: no file processing routines are used inside MeasurementProcessor() all the file processing routines are in the main function
18 | So the data read/write is decoupled from the algorithm
19 | 4) The results are saved in an output file:
20 | "data/obj_pose-laser-radar-ekf-output.txt"
21 | 
22 | Output file format:
23 | est_px est_py est_vx est_vy meas_px meas_py gt_px gt_py gt_vx gt_vy
24 | 
25 | Example:
26 | 4.53271	0.279	-0.842172	53.1339	4.29136	0.215312	2.28434	0.226323
27 | 43.2222	2.65959	0.931181	23.2469	4.29136	0.215312	2.28434	0.226323
28 | 
29 |  


--------------------------------------------------------------------------------
/include/kalman_filter/kalman_filter.h:
--------------------------------------------------------------------------------
 1 | #ifndef KALMAN_FILTER_H_
 2 | #define KALMAN_FILTER_H_
 3 | #include "Eigen/Dense"
 4 | 
 5 | class KalmanFilter {
 6 |  public:
 7 | 
 8 |   // state vector
 9 |   Eigen::VectorXd x_;
10 | 
11 |   // state covariance matrix
12 |   Eigen::MatrixXd P_;
13 | 
14 |   // state transistion matrix
15 |   Eigen::MatrixXd F_;
16 | 
17 |   // process covariance matrix
18 |   Eigen::MatrixXd Q_;
19 | 
20 |   // measurement matrix
21 |   Eigen::MatrixXd H_;
22 | 
23 |   // measurement covariance matrix
24 |   Eigen::MatrixXd R_;
25 | 
26 |   /**
27 |    * Constructor
28 |    */
29 |   KalmanFilter();
30 | 
31 |   /**
32 |    * Destructor
33 |    */
34 |   virtual ~KalmanFilter();
35 | 
36 |   /**
37 |    * Init Initializes Kalman filter
38 |    * @param x_in Initial state
39 |    * @param P_in Initial state covariance
40 |    * @param F_in Transition matrix
41 |    * @param H_in Measurement matrix
42 |    * @param R_in Measurement covariance matrix
43 |    * @param Q_in Process covariance matrix
44 |    */
45 |   void Init(Eigen::VectorXd &x_in, Eigen::MatrixXd &P_in, Eigen::MatrixXd &F_in,
46 |             Eigen::MatrixXd &H_in, Eigen::MatrixXd &R_in, Eigen::MatrixXd &Q_in);
47 | 
48 |   /**
49 |    * Prediction Predicts the state and the state covariance
50 |    * using the process model
51 |    * @param delta_T Time between k and k+1 in s
52 |    */
53 |   void Predict();
54 | 
55 |   /**
56 |    * Updates the state by using standard Kalman Filter equations
57 |    * @param z The measurement at k+1
58 |    */
59 |   void Update(const Eigen::VectorXd &z);
60 | 
61 |   /**
62 |    * Updates the state by using Extended Kalman Filter equations
63 |    * @param z The measurement at k+1
64 |    */
65 |   void UpdateEKF(const Eigen::VectorXd &z);
66 | 
67 |   /**
68 |    * Universal update Kalman Filter step. Equations from the lectures
69 |    * @param y The error
70 |    */
71 |   void KF(const Eigen::VectorXd &y);
72 | 
73 | };
74 | 
75 | #endif /* KALMAN_FILTER_H_ */
76 | 


--------------------------------------------------------------------------------
/src/kalman_filter.cpp:
--------------------------------------------------------------------------------
 1 | #include "kalman_filter/kalman_filter.h"
 2 | 
 3 | using Eigen::MatrixXd;
 4 | using Eigen::VectorXd;
 5 | 
 6 | KalmanFilter::KalmanFilter() {}
 7 | 
 8 | KalmanFilter::~KalmanFilter() {}
 9 | 
10 | void KalmanFilter::Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,
11 |                         MatrixXd &H_in, MatrixXd &R_in, MatrixXd &Q_in) {
12 |   x_ = x_in; // object state
13 |   P_ = P_in; // object covariance matrix
14 |   F_ = F_in; // state transition matrix
15 |   H_ = H_in; // measurement matrix
16 |   R_ = R_in; // measurement covariance matrix
17 |   Q_ = Q_in; // process covariance matrix
18 | }
19 | 
20 | // The Kalman filter predict function. The same for linear and extended Kalman filter
21 | void KalmanFilter::Predict() {
22 |   x_ = F_ * x_; // There is no external motion, so, we do not have to add "+u"
23 |   MatrixXd Ft = F_.transpose();
24 |   P_ = F_ * P_ * Ft + Q_;
25 | }
26 | 
27 | void KalmanFilter::Update(const VectorXd &z) {
28 |   // Kalman filter update step. Equations from the lectures
29 |   VectorXd y = z - H_ * x_; // 残差：当前观测 - H(X)*根据上一帧预测的状态量 = 当前观测 - 预测得到的观测
30 |   KF(y);
31 | }
32 | 
33 | void KalmanFilter::UpdateEKF(const VectorXd &z) {
34 | 
35 |   // 预测的状态量->预测的观测
36 |   double rho = sqrt(x_(0) * x_(0) + x_(1) * x_(1));
37 |   double theta = atan(x_(1) / x_(0));
38 |   double rho_dot = (x_(0) * x_(2) + x_(1) * x_(3)) / rho;
39 |   VectorXd h = VectorXd(3); // h(x_)
40 |   h << rho, theta, rho_dot;
41 | 
42 |   VectorXd y = z - h; // 观测空间： 计算状态量残差 = 观测 - 预测
43 |   // Calculations are essentially the same to the Update function
44 |   KF(y);
45 | }
46 | 
47 | // Universal update Kalman Filter step. Equations from the lectures
48 | void KalmanFilter::KF(const VectorXd &y) {
49 |   // 观测空间
50 |   // 根据观测的残差计算残差的协方差矩阵、卡尔曼增益
51 |   MatrixXd Ht = H_.transpose(); // H-1
52 |   MatrixXd S = H_ * P_ * Ht + R_; // 观测残差的协方差矩阵
53 |   MatrixXd Si = S.inverse();
54 |   MatrixXd K = P_ * Ht * Si;  // 卡尔安增益， 预测的协方差/观测残差的协方差，即预测的权重占比
55 | 
56 |   // 更新当前系统状态
57 |   x_ = x_ + (K * y);  // 当前状态 = 预测状态 + 权重 * 预测在残差权重的占比
58 |   int x_size = x_.size();
59 |   MatrixXd I = MatrixXd::Identity(x_size, x_size);
60 |   P_ = (I - K * H_) * P_;   // 当前协方差 = （1-K）* 预测的协方差
61 | }
62 | 


--------------------------------------------------------------------------------
/src/tools.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include "kalman_filter/tools.h"
 3 | 
 4 | #define EPS 0.0001 // A very small number
 5 | #define EPS2 0.0000001
 6 | 
 7 | using namespace std;
 8 | using Eigen::VectorXd;
 9 | using Eigen::MatrixXd;
10 | using std::vector;
11 | 
12 | Tools::Tools() {}
13 | 
14 | Tools::~Tools() {}
15 | 
16 | VectorXd Tools::CalculateRMSE(const vector<VectorXd> &estimations,
17 |                               const vector<VectorXd> &ground_truth) {
18 |   VectorXd rmse(4);
19 |   rmse << 0,0,0,0;
20 |   // Check the validity of the following inputs:
21 |   // The estimation vector size should not be zero
22 |   if(estimations.size() == 0){
23 |     cout << "Input is empty" << endl;
24 |     return rmse;
25 |   }
26 |   // The estimation vector size should equal ground truth vector size
27 |   if(estimations.size() != ground_truth.size()){
28 |     cout << "Invalid estimation or ground_truth. Data should have the same size" << endl;
29 |     return rmse;
30 |   }
31 |   // Accumulate squared residuals
32 |   for(unsigned int i=0; i < estimations.size(); ++i){
33 |     VectorXd residual = estimations[i] - ground_truth[i];
34 |     // Coefficient-wise multiplication
35 |     residual = residual.array()*residual.array();
36 |     rmse += residual;
37 |   }
38 | 
39 |   // Calculate the mean
40 |   rmse = rmse / estimations.size();
41 |   rmse = rmse.array().sqrt();
42 |   return rmse;
43 | }
44 | 
45 | MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) {
46 |   // Code from lectures quizes
47 |   float px = x_state(0);
48 |   float py = x_state(1);
49 |   float vx = x_state(2);
50 |   float vy = x_state(3);
51 |   MatrixXd Hj(3,4);
52 |   // Deal with the special case problems
53 |   if (fabs(px) < EPS and fabs(py) < EPS){
54 | 	  px = EPS;
55 | 	  py = EPS;
56 |   }
57 |   // Pre-compute a set of terms to avoid repeated calculation
58 |   float c1 = px*px+py*py;
59 |   // Check division by zero
60 |   if(fabs(c1) < EPS2){
61 | 	c1 = EPS2;
62 |   }
63 |   float c2 = sqrt(c1);
64 |   float c3 = (c1*c2);
65 |   // Compute the Jacobian matrix
66 |   Hj << (px/c2), (py/c2), 0, 0,
67 |        -(py/c1), (px/c1), 0, 0,
68 |         py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2;
69 |   return Hj;
70 | }
71 | 


--------------------------------------------------------------------------------
/task.md:
--------------------------------------------------------------------------------
 1 | # Extended Kalman Filter Project Starter Code
 2 | Self-Driving Car Engineer Nanodegree Program
 3 | 
 4 | ---
 5 | 
 6 | ## Dependencies
 7 | 
 8 | * cmake >= 3.5
 9 |  * All OSes: [click here for installation instructions](https://cmake.org/install/)
10 | * make >= 4.1
11 |   * Linux: make is installed by default on most Linux distros
12 |   * Mac: [install Xcode command line tools to get make](https://developer.apple.com/xcode/features/)
13 |   * Windows: [Click here for installation instructions](http://gnuwin32.sourceforge.net/packages/make.htm)
14 | * gcc/g++ >= 5.4
15 |   * Linux: gcc / g++ is installed by default on most Linux distros
16 |   * Mac: same deal as make - [install Xcode command line tools]((https://developer.apple.com/xcode/features/)
17 |   * Windows: recommend using [MinGW](http://www.mingw.org/)
18 | 
19 | ## Basic Build Instructions
20 | 
21 | 1. Clone this repo.
22 | 2. Make a build directory: `mkdir build && cd build`
23 | 3. Compile: `cmake .. && make` 
24 |    * On windows, you may need to run: `cmake .. -G "Unix Makefiles" && make`
25 | 4. Run it: `./ExtendedKF path/to/input.txt path/to/output.txt`. You can find
26 |    some sample inputs in 'data/'.
27 |     - eg. `./ExtendedKF ../data/sample-laser-radar-measurement-data-1.txt output.txt`
28 | 
29 | ## Editor Settings
30 | 
31 | We've purposefully kept editor configuration files out of this repo in order to
32 | keep it as simple and environment agnostic as possible. However, we recommend
33 | using the following settings:
34 | 
35 | * indent using spaces
36 | * set tab width to 2 spaces (keeps the matrices in source code aligned)
37 | 
38 | ## Code Style
39 | 
40 | Please (do your best to) stick to [Google's C++ style guide](https://google.github.io/styleguide/cppguide.html).
41 | 
42 | ## Generating Additional Data
43 | 
44 | This is optional!
45 | 
46 | If you'd like to generate your own radar and lidar data, see the
47 | [utilities repo](https://github.com/udacity/CarND-Mercedes-SF-Utilities) for
48 | Matlab scripts that can generate additional data.
49 | 
50 | ## Project Instructions and Rubric
51 | 
52 | Note: regardless of the changes you make, your project must be buildable using
53 | cmake and make!
54 | 
55 | More information is only accessible by people who are already enrolled in Term 2
56 | of CarND. If you are enrolled, see [the project page](https://classroom.udacity.com/nanodegrees/nd013/parts/40f38239-66b6-46ec-ae68-03afd8a601c8/modules/0949fca6-b379-42af-a919-ee50aa304e6a/lessons/f758c44c-5e40-4e01-93b5-1a82aa4e044f/concepts/12dd29d8-2755-4b1b-8e03-e8f16796bea8)
57 | for instructions and the project rubric.
58 | 
59 | ## Hints!
60 | 
61 | * You don't have to follow this directory structure, but if you do, your work
62 |   will span all of the .cpp files here. Keep an eye out for TODOs.
63 | 
64 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>kalman_filter</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The kalman_filter package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="xchu@todo.todo">xchu</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/kalman_filter</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>roscpp</build_depend>
53 |   <build_depend>rospy</build_depend>
54 |   <build_depend>sensor_msgs</build_depend>
55 |   <build_depend>std_msgs</build_depend>
56 |   <build_export_depend>roscpp</build_export_depend>
57 |   <build_export_depend>rospy</build_export_depend>
58 |   <build_export_depend>sensor_msgs</build_export_depend>
59 |   <build_export_depend>std_msgs</build_export_depend>
60 |   <exec_depend>roscpp</exec_depend>
61 |   <exec_depend>rospy</exec_depend>
62 |   <exec_depend>sensor_msgs</exec_depend>
63 |   <exec_depend>std_msgs</exec_depend>
64 | 
65 | 
66 |   <!-- The export tag contains other, unspecified, tags -->
67 |   <export>
68 |     <!-- Other tools can request additional information be placed here -->
69 | 
70 |   </export>
71 | </package>
72 | 


--------------------------------------------------------------------------------
/include/kalman_filter/fusion_ukf.h:
--------------------------------------------------------------------------------
  1 | #ifndef UKF_H
  2 | #define UKF_H
  3 | 
  4 | #include "measurement_package.h"
  5 | #include "Eigen/Dense"
  6 | #include <vector>
  7 | #include <string>
  8 | #include <fstream>
  9 | #include "tools.h"
 10 | 
 11 | using Eigen::MatrixXd;
 12 | using Eigen::VectorXd;
 13 | 
 14 | class UKF {
 15 | public:
 16 | 
 17 |   ///* initially set to false, set to true in first call of ProcessMeasurement
 18 |   bool is_initialized_;
 19 | 
 20 |   ///* if this is false, laser measurements will be ignored (except for init)
 21 |   bool use_laser_;
 22 | 
 23 |   ///* if this is false, radar measurements will be ignored (except for init)
 24 |   bool use_radar_;
 25 | 
 26 |   ///* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
 27 |   VectorXd x_;
 28 | 
 29 |   ///* state covariance matrix
 30 |   MatrixXd P_;
 31 | 
 32 |   ///* predicted sigma points matrix
 33 |   MatrixXd Xsig_pred_;
 34 | 
 35 |   ///* time when the state is true, in us
 36 |   long long time_us_;
 37 | 
 38 |   ///* Process noise standard deviation longitudinal acceleration in m/s^2
 39 |   double std_a_;
 40 | 
 41 |   ///* Process noise standard deviation yaw acceleration in rad/s^2
 42 |   double std_yawdd_;
 43 | 
 44 |   ///* Laser measurement noise standard deviation position1 in m
 45 |   double std_laspx_;
 46 | 
 47 |   ///* Laser measurement noise standard deviation position2 in m
 48 |   double std_laspy_;
 49 | 
 50 |   ///* Radar measurement noise standard deviation radius in m
 51 |   double std_radr_;
 52 | 
 53 |   ///* Radar measurement noise standard deviation angle in rad
 54 |   double std_radphi_;
 55 | 
 56 |   ///* Radar measurement noise standard deviation radius change in m/s
 57 |   double std_radrd_ ;
 58 | 
 59 |   ///* Weights of sigma points
 60 |   VectorXd weights_;
 61 | 
 62 |   ///* State dimension
 63 |   int n_x_;
 64 | 
 65 |   ///* Augmented state dimension
 66 |   int n_aug_;
 67 |   
 68 |   ///* Number of sigma points
 69 |   int n_sig_;
 70 | 
 71 |   ///* Sigma point spreading parameter
 72 |   double lambda_;
 73 | 
 74 |   ///* the current NIS for radar
 75 |   double NIS_radar_;
 76 | 
 77 |   ///* the current NIS for laser
 78 |   double NIS_laser_;
 79 |   
 80 |   ///* Radar measurement noise covariance matrix
 81 |   MatrixXd R_radar_;
 82 |   
 83 |   ///* Lidar measurement noise covariance matrix
 84 |   MatrixXd R_lidar_;
 85 |   
 86 |   /**
 87 |    * Constructor
 88 |    */
 89 |   UKF();
 90 | 
 91 |   /**
 92 |    * Destructor
 93 |    */
 94 |   virtual ~UKF();
 95 | 
 96 |   /**
 97 |    *  Angle normalization to [-Pi, Pi]
 98 |    */
 99 |   void NormAng(double *ang);
100 |   
101 |   /**
102 |    * ProcessMeasurement
103 |    * @param measurement_pack The latest measurement data of either radar or laser
104 |    */
105 |   void ProcessMeasurement(MeasurementPackage measurement_pack);
106 | 
107 |   /**
108 |    * Prediction Predicts sigma points, the state, and the state covariance
109 |    * matrix
110 |    * @param delta_t Time between k and k+1 in s
111 |    */
112 |   void Prediction(double delta_t);
113 | 
114 |   /**
115 |    * Updates the state and the state covariance matrix using a laser measurement
116 |    * @param meas_package The measurement at k+1
117 |    */
118 |   void UpdateLidar(MeasurementPackage meas_package);
119 | 
120 |   /**
121 |    * Updates the state and the state covariance matrix using a radar measurement
122 |    * @param meas_package The measurement at k+1
123 |    */
124 |   void UpdateRadar(MeasurementPackage meas_package);
125 |   /**
126 |    * Updates the state and the state covariance matrix of the UKF
127 |    * 
128 |    */
129 |   void UpdateUKF(MeasurementPackage meas_package, MatrixXd Zsig, int n_z);
130 | };
131 | 
132 | #endif /* UKF_H */
133 | 


--------------------------------------------------------------------------------
/src/fusion_ekf.cpp:
--------------------------------------------------------------------------------
  1 | #include "kalman_filter/fusion_ekf.h"
  2 | #include "kalman_filter/tools.h"
  3 | #include "Eigen/Dense"
  4 | #include <iostream>
  5 | #define EPS 0.0001 // A very small number
  6 | 
  7 | using namespace std;
  8 | using Eigen::MatrixXd;
  9 | using Eigen::VectorXd;
 10 | using std::vector;
 11 | 
 12 | /*
 13 |  * Constructor.
 14 |  */
 15 | FusionEKF::FusionEKF() {
 16 |   /**
 17 |   TODO:
 18 |     * Finish initializing the FusionEKF.
 19 |     * Set the process and measurement noises
 20 |     * 系统状态量：px, py, vx, vy
 21 |     * 观测量：
 22 |     *   1.激光雷达：px, py ,可直接作为系统状态量使用, 无需转换(H矩阵)
 23 |     *   2.毫米波雷达：rho(距离), phi(角度), rho_dot(角度变化率), 需要先转换成系统状态量使用(非线性过程)
 24 |     *  两个步骤：
 25 |     *    1.预测：根据运动模型预测下一时刻的系统状态 (px py vx vy), x_k = F(x)*x_k-1+Q
 26 |     *           系统上一时刻状态->系统当前时刻状态, 不涉及状态空间与观测空间的转换, 即不考虑传感器类型
 27 |     *           这个过程是线性的, 其状态转换矩阵定义为F(x), 过程噪声定义为Q
 28 |     *    2.测量更新： 根据当前的观测数据, 去修正预测的结果
 29 |     *           这里首先需要将预测数据转换到测量空间下, 这个过程涉及到具体的传感器模型, 以及观测带来的传感器本身的噪声R
 30 |     *           z_k = H(x)*x_k + R  H矩阵则是描述这个状态空间到观测空间的转换
 31 |     *           这样计算残差 delta_x = z_k - x_k
 32 |     *
 33 |     *
 34 |   */
 35 |   is_initialized_ = false;
 36 |   previous_timestamp_ = 0;
 37 |   // Initializing matrices
 38 |   // 激光雷达与毫米波的观测协方差矩阵R
 39 |   R_laser_ = MatrixXd(2, 2);
 40 |   R_radar_ = MatrixXd(3, 3);
 41 |   // 激光雷达、毫米波状态空间->观测空间的变换矩阵H
 42 |   H_laser_ = MatrixXd(2, 4);
 43 |   H_laser_ << 1, 0, 0, 0,
 44 | 			  0, 1, 0, 0;
 45 |   // 毫米波的状态变换矩阵(雅克比), 因为是非线性
 46 |   Hj_ = MatrixXd(3, 4);
 47 | 
 48 |   // 激光雷达和毫米波的测量噪声R是设备固有的,在说明中已给出
 49 |   R_laser_ << 0.0225, 0,
 50 |               0, 0.0225;
 51 |   R_radar_ << 0.09, 0, 0,
 52 |               0, 0.0009, 0,
 53 |               0, 0, 0.09;
 54 | }
 55 | 
 56 | /**
 57 | * Destructor.
 58 | */
 59 | FusionEKF::~FusionEKF() {}
 60 | 
 61 | void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
 62 | 
 63 |   /*****************************************************************************
 64 |    * TODO: Initialization
 65 |    *  利用第一帧数据, 初始化系统状态变量 ekf_.x_,协方差矩阵P,状态量(px, py, vx, vy)
 66 |    *  这里传感器不同, 可以初始化的变量就不同, 比如激光雷达只能初始化px和py
 67 |    *  第二帧之后进来的数据则直接进入预测流程
 68 |    ****************************************************************************/
 69 |   if (!is_initialized_) {
 70 |     ekf_.x_ = VectorXd(4);
 71 |     if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
 72 |       // 利用第一帧毫米波数据初始化状态量
 73 |       float rho = measurement_pack.raw_measurements_[0]; // range
 74 | 	  float phi = measurement_pack.raw_measurements_[1]; // bearing
 75 | 	  float rho_dot = measurement_pack.raw_measurements_[2]; // velocity of rho
 76 | 	  // 毫米波数据可同时初始化所有的系统状态变量
 77 | 	  float x = rho * cos(phi); 
 78 | 	  float y = rho * sin(phi);
 79 | 	  float vx = rho_dot * cos(phi);
 80 | 	  float vy = rho_dot * sin(phi);
 81 | 	  ekf_.x_ << x, y, vx , vy;
 82 |     }
 83 |     else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
 84 |       // 激光只能初始化px和py
 85 |       ekf_.x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
 86 |     }
 87 |     // 这里校验状态量, 状态量xy太小的话设为EPS
 88 |     if (fabs(ekf_.x_(0)) < EPS and fabs(ekf_.x_(1)) < EPS){
 89 | 		ekf_.x_(0) = EPS;
 90 | 		ekf_.x_(1) = EPS;
 91 | 	}
 92 | 	// 初始化系统4*4的协方差矩阵P,因为我们不知道速度,所以将vx,vy方差设置大一些
 93 |     ekf_.P_ = MatrixXd(4, 4);
 94 |     ekf_.P_ << 1, 0, 0, 0,
 95 | 			   0, 1, 0, 0,
 96 | 			   0, 0, 1000, 0,
 97 | 			   0, 0, 0, 1000;
 98 |     // Print the initialization results
 99 |     cout << "EKF init: " << ekf_.x_ << endl;
100 |     // 更新时间戳，后面需要计算delta_t
101 |     previous_timestamp_ = measurement_pack.timestamp_;
102 |     is_initialized_ = true;
103 |     return;
104 |   }
105 | 
106 |   /**
107 |    TODO: Prediction
108 |      * 根据上一帧的系统状态(px_1, py_1, vx_1, vy_1)预测当前帧的状态(px py vx vy)
109 |      * 计算状态转换矩阵F->当前预测的状态量x
110 |      * 预测的过程噪声Q, 一部分噪声是由匀速运动模型带来的
111 |    */
112 |   // 计算两帧的时间差dt
113 |   float dt = (measurement_pack.timestamp_ - previous_timestamp_);
114 |   dt /= 1000000.0; // convert micros to s
115 |   previous_timestamp_ = measurement_pack.timestamp_;
116 |   
117 |   // 根据匀速运动模型，设置状态变换的F矩阵
118 |   // px = px_1 + vx*dt
119 |   // py = py_1 + vy*dt
120 |   ekf_.F_ = MatrixXd(4, 4);
121 |   ekf_.F_ << 1, 0, dt, 0,
122 | 			0, 1, 0, dt,
123 | 			0, 0, 1, 0,
124 | 			0, 0, 0, 1;
125 |   // 计算随机加速度噪声协方差，需要根据实际的运动模型进行估计
126 |   float noise_ax = 9.0;
127 |   float noise_ay = 9.0;
128 |   // 根据上一时刻状态->预测当前状态,过程噪声协方差矩阵Q
129 |   float dt_2 = dt * dt; //dt^2
130 |   float dt_3 = dt_2 * dt; //dt^3
131 |   float dt_4 = dt_3 * dt; //dt^4
132 |   float dt_4_4 = dt_4 / 4; //dt^4/4
133 |   float dt_3_2 = dt_3 / 2; //dt^3/2
134 |   ekf_.Q_ = MatrixXd(4, 4);
135 |   ekf_.Q_ << dt_4_4 * noise_ax, 0, dt_3_2 * noise_ax, 0,
136 | 	         0, dt_4_4 * noise_ay, 0, dt_3_2 * noise_ay,
137 | 	         dt_3_2 * noise_ax, 0, dt_2 * noise_ax, 0,
138 |  	         0, dt_3_2 * noise_ay, 0, dt_2 * noise_ay;
139 | 
140 |   ekf_.Predict(); // 输入转换矩阵F，过程噪声Q
141 | 
142 |   /**
143 |    TODO: Update
144 |      * 根据当前不同类型的观测数据和上一帧预测的状态，来计算残差和卡尔曼增益.
145 |      * 获取当前的状态量以及协方差矩阵.
146 |    */
147 | 
148 |   if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
149 |     // 当前你观测是毫米波数据的话，H(X)是从状态空间4->测量空间的转换3
150 | 	// H(X)是非线性的，观测空间->状态空间的变换之后，已经不一定服从高斯分布了
151 | 	// 所以对H(X)用泰勒一阶展开,因此H矩阵可以由雅克比矩阵J代替
152 | 	ekf_.H_ = tools.CalculateJacobian(ekf_.x_); // 测量变量3个，状态变量4个，雅克比3*4
153 | 	ekf_.R_ = R_radar_;
154 | 	ekf_.UpdateEKF(measurement_pack.raw_measurements_);
155 |   } else {
156 |     // 当前观测是激光雷达数据的话，就是线性变换了
157 |     // 按卡尔曼滤波模型更新测量
158 | 	ekf_.H_ = H_laser_;
159 | 	ekf_.R_ = R_laser_;
160 | 	ekf_.Update(measurement_pack.raw_measurements_);
161 |   }
162 |   // print the output
163 |   cout << "x_ = " << ekf_.x_ << endl;
164 |   cout << "P_ = " << ekf_.P_ << endl;
165 | }
166 | 


--------------------------------------------------------------------------------
/src/main_ekf.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | 
  3 | #include "kalman_filter/fusion_ekf.h"
  4 | #include "kalman_filter/ground_truth_package.h"
  5 | #include "kalman_filter/measurement_package.h"
  6 | 
  7 | #include "Eigen/Dense"
  8 | #include "ros/ros.h"
  9 | #include <fstream>
 10 | #include <iostream>
 11 | #include <vector>
 12 | #include <cstdlib>
 13 | 
 14 | using namespace std;
 15 | using Eigen::MatrixXd;
 16 | using Eigen::VectorXd;
 17 | using std::vector;
 18 | 
 19 | void check_arguments(int argc, char *argv[]) {
 20 |   string usage_instructions = "Usage instructions: ";
 21 |   usage_instructions += argv[0];
 22 |   usage_instructions += " path/to/input.txt output.txt";
 23 | 
 24 |   bool has_valid_args = false;
 25 | 
 26 |   // make sure the user has provided input and output files
 27 |   if (argc == 1) {
 28 |     cerr << usage_instructions << endl;
 29 |   } else if (argc == 2) {
 30 |     cerr << "Please include an output file.\n" << usage_instructions << endl;
 31 |   } else if (argc == 3) {
 32 |     has_valid_args = true;
 33 |   } else if (argc > 3) {
 34 |     cerr << "Too many arguments.\n" << usage_instructions << endl;
 35 |   }
 36 | 
 37 |   if (!has_valid_args) {
 38 |     exit(EXIT_FAILURE);
 39 |   }
 40 | }
 41 | 
 42 | void check_files(ifstream &in_file, string &in_name,
 43 |                  ofstream &out_file, string &out_name) {
 44 |   if (!in_file.is_open()) {
 45 |     cerr << "Cannot open input file: " << in_name << endl;
 46 |     exit(EXIT_FAILURE);
 47 |   }
 48 | 
 49 |   if (!out_file.is_open()) {
 50 |     cerr << "Cannot open output file: " << out_name << endl;
 51 |     exit(EXIT_FAILURE);
 52 |   }
 53 | }
 54 | 
 55 | int main(int argc, char **argv) {
 56 | 
 57 |   ros::init(argc, argv, "kalmanfilter");
 58 |   ros::NodeHandle node_handle;
 59 | 
 60 |   // check参数
 61 | //  check_arguments(argc, argv);
 62 | //  string in_file_name_ = argv[1];
 63 | //  string out_file_name_ = argv[2];
 64 | 
 65 |   string in_file_name_ =
 66 |       "/home/xchu/workspace/hdmap_testws/src/kalman_filter/data/sample-laser-radar-measurement-data-1.txt";
 67 |   string out_file_name_ = "/home/xchu/workspace/hdmap_testws/src/kalman_filter/data/ekf_out.txt";
 68 | 
 69 |   ifstream in_file_(in_file_name_.c_str(), ifstream::in);
 70 |   ofstream out_file_(out_file_name_.c_str(), ofstream::out);
 71 | 
 72 |   // 校验文件路径
 73 |   check_files(in_file_, in_file_name_, out_file_, out_file_name_);
 74 | 
 75 |   // measurement_pack_list 中包含一个时间戳和两种类型的数据，按时间戳顺序排列。用来存储读取的数据文件
 76 |   vector<MeasurementPackage> measurement_pack_list;
 77 |   vector<GroundTruthPackage> gt_pack_list;
 78 | 
 79 |   // 逐行读取毫米波和激光雷达数据
 80 |   string line;
 81 |   while (getline(in_file_, line)) {
 82 |     string sensor_type;
 83 |     MeasurementPackage meas_package;
 84 |     GroundTruthPackage gt_package;
 85 |     istringstream iss(line);
 86 |     long long timestamp;
 87 | 
 88 |     // 读取激光雷达、毫米波、groundTruth数据
 89 |     iss >> sensor_type;  // 数据的第一行
 90 |     if (sensor_type.compare("L") == 0) {
 91 |       // LASER MEASUREMENT
 92 |       // 激光雷达数据, 只有px,py
 93 |       // read measurements at this timestamp
 94 |       meas_package.sensor_type_ = MeasurementPackage::LASER;
 95 |       meas_package.raw_measurements_ = VectorXd(2);
 96 |       float x;
 97 |       float y;
 98 |       iss >> x;
 99 |       iss >> y;
100 |       meas_package.raw_measurements_ << x, y;
101 |       iss >> timestamp;
102 |       meas_package.timestamp_ = timestamp;
103 |       measurement_pack_list.push_back(meas_package);
104 |     } else if (sensor_type.compare("R") == 0) {
105 |       // RADAR MEASUREMENT
106 |       // 毫米波数据, 包含ro, phi, ro_dot
107 |       meas_package.sensor_type_ = MeasurementPackage::RADAR;
108 |       meas_package.raw_measurements_ = VectorXd(3);
109 |       float ro;
110 |       float phi;
111 |       float ro_dot;
112 |       iss >> ro;
113 |       iss >> phi;
114 |       iss >> ro_dot;
115 |       meas_package.raw_measurements_ << ro, phi, ro_dot;
116 |       iss >> timestamp;
117 |       meas_package.timestamp_ = timestamp;
118 |       measurement_pack_list.push_back(meas_package);
119 |     }
120 | 
121 |     // ground truth data
122 |     float x_gt;
123 |     float y_gt;
124 |     float vx_gt;
125 |     float vy_gt;
126 |     iss >> x_gt;
127 |     iss >> y_gt;
128 |     iss >> vx_gt;
129 |     iss >> vy_gt;
130 |     gt_package.gt_values_ = VectorXd(4);
131 |     gt_package.gt_values_ << x_gt, y_gt, vx_gt, vy_gt;
132 |     gt_pack_list.push_back(gt_package);
133 |   }
134 | 
135 |   // 初始化EKF参数
136 |   // EKF主要包括两个矩阵, 两个函数
137 |   // 状态预测函数,需要计算状态变换的F矩阵；
138 |   // 测量更新函数,需要计算测量变换矩阵H
139 |   FusionEKF fusionEKF;
140 | 
141 |   vector<VectorXd> estimations;
142 |   vector<VectorXd> ground_truth;
143 | 
144 |   //Call the EKF-based fusion
145 |   size_t N = measurement_pack_list.size();
146 |   for (size_t k = 0; k < N; ++k) {
147 | 
148 |     // 第一帧数据初始化预测的X,P和转换矩阵F
149 |     // 从第二帧数据起开始进行融合 (the speed is unknown in the first frame)
150 |     fusionEKF.ProcessMeasurement(measurement_pack_list[k]);
151 | 
152 |     // 将预测状态量结果输出到文件
153 |     out_file_ << fusionEKF.ekf_.x_(0) << "\t";
154 |     out_file_ << fusionEKF.ekf_.x_(1) << "\t";
155 |     out_file_ << fusionEKF.ekf_.x_(2) << "\t";
156 |     out_file_ << fusionEKF.ekf_.x_(3) << "\t";
157 | 
158 |     // 将观测结果输出到文件
159 |     if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::LASER) {
160 |       // output the estimation
161 |       out_file_ << measurement_pack_list[k].raw_measurements_(0) << "\t";
162 |       out_file_ << measurement_pack_list[k].raw_measurements_(1) << "\t";
163 |     } else if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::RADAR) {
164 |       // output the estimation in the cartesian coordinates
165 |       float ro = measurement_pack_list[k].raw_measurements_(0);
166 |       float phi = measurement_pack_list[k].raw_measurements_(1);
167 |       out_file_ << ro * cos(phi) << "\t"; // p1_meas
168 |       out_file_ << ro * sin(phi) << "\t"; // ps_meas
169 |     }
170 | 
171 |     // 输出ground truth
172 |     out_file_ << gt_pack_list[k].gt_values_(0) << "\t";
173 |     out_file_ << gt_pack_list[k].gt_values_(1) << "\t";
174 |     out_file_ << gt_pack_list[k].gt_values_(2) << "\t";
175 |     out_file_ << gt_pack_list[k].gt_values_(3) << "\n";
176 | 
177 |     estimations.push_back(fusionEKF.ekf_.x_);
178 |     ground_truth.push_back(gt_pack_list[k].gt_values_);
179 |   }
180 | 
181 |   // 计算RMSE
182 |   Tools tools;
183 |   cout << "Accuracy - RMSE:" << endl << tools.CalculateRMSE(estimations, ground_truth) << endl;
184 | 
185 |   // close files
186 |   if (out_file_.is_open()) {
187 |     out_file_.close();
188 |   }
189 |   if (in_file_.is_open()) {
190 |     in_file_.close();
191 |   }
192 | 
193 |   return 0;
194 | }
195 | 


--------------------------------------------------------------------------------
/src/main_ukf.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * Author: xc Hu
  3 |  * Date: 2020/8/12 下午2:15
  4 |  * Content:
  5 |  * Email: 2022087641@qq.com
  6 |  */
  7 | 
  8 | #include <fstream>
  9 | #include <iostream>
 10 | #include <vector>
 11 | #include <stdlib.h>
 12 | #include "Eigen/Dense"
 13 | #include "kalman_filter/fusion_ukf.h"
 14 | #include "kalman_filter/ground_truth_package.h"
 15 | #include "kalman_filter/measurement_package.h"
 16 | #include "ros/ros.h"
 17 | using namespace std;
 18 | using Eigen::MatrixXd;
 19 | using Eigen::VectorXd;
 20 | using std::vector;
 21 | 
 22 | void check_arguments(int argc, char *argv[]) {
 23 |   string usage_instructions = "Usage instructions: ";
 24 |   usage_instructions += argv[0];
 25 |   usage_instructions += " path/to/input.txt output.txt";
 26 | 
 27 |   bool has_valid_args = false;
 28 | 
 29 |   // make sure the user has provided input and output files
 30 |   if (argc == 1) {
 31 |     cerr << usage_instructions << endl;
 32 |   } else if (argc == 2) {
 33 |     cerr << "Please include an output file.\n" << usage_instructions << endl;
 34 |   } else if (argc == 3) {
 35 |     has_valid_args = true;
 36 |   } else if (argc > 3) {
 37 |     cerr << "Too many arguments.\n" << usage_instructions << endl;
 38 |   }
 39 | 
 40 |   if (!has_valid_args) {
 41 |     exit(EXIT_FAILURE);
 42 |   }
 43 | }
 44 | 
 45 | void check_files(ifstream &in_file, string &in_name,
 46 |                  ofstream &out_file, string &out_name) {
 47 |   if (!in_file.is_open()) {
 48 |     cerr << "Cannot open input file: " << in_name << endl;
 49 |     exit(EXIT_FAILURE);
 50 |   }
 51 | 
 52 |   if (!out_file.is_open()) {
 53 |     cerr << "Cannot open output file: " << out_name << endl;
 54 |     exit(EXIT_FAILURE);
 55 |   }
 56 | }
 57 | 
 58 | int main(int argc, char *argv[]) {
 59 | 
 60 | //  check_arguments(argc, argv);
 61 | //  string in_file_name_ = argv[1];
 62 | //  ifstream in_file_(in_file_name_.c_str(), ifstream::in);
 63 | //  string out_file_name_ = argv[2];
 64 | //  ofstream out_file_(out_file_name_.c_str(), ofstream::out);
 65 | //  check_files(in_file_, in_file_name_, out_file_, out_file_name_);
 66 | 
 67 |   ros::init(argc, argv, "ukalmanfilter");
 68 |   ros::NodeHandle node_handle;
 69 | 
 70 |   string in_file_name_ =
 71 |       "/home/xchu/workspace/hdmap_testws/src/kalman_filter/data/sample-laser-radar-measurement-data-1.txt";
 72 |   string out_file_name_ = "/home/xchu/workspace/hdmap_testws/src/kalman_filter/data/ukf_out.txt";
 73 | 
 74 |   ifstream in_file_(in_file_name_.c_str(), ifstream::in);
 75 |   ofstream out_file_(out_file_name_.c_str(), ofstream::out);
 76 | 
 77 |   check_files(in_file_, in_file_name_, out_file_, out_file_name_);
 78 | 
 79 |   /**********************************************
 80 |    *  Set Measurements                          *
 81 |    **********************************************/
 82 | 
 83 |   vector<MeasurementPackage> measurement_pack_list;
 84 |   vector<GroundTruthPackage> gt_pack_list;
 85 | 
 86 |   string line;
 87 | 
 88 |   // 读数据和ground truth
 89 |   while (getline(in_file_, line)) {
 90 |     string sensor_type;
 91 |     MeasurementPackage meas_package;
 92 |     GroundTruthPackage gt_package;
 93 |     istringstream iss(line);
 94 |     long long timestamp;
 95 | 
 96 |     // reads first element from the current line
 97 |     iss >> sensor_type;
 98 | 
 99 |     if (sensor_type.compare("L") == 0) {
100 |       // laser measurement
101 | 
102 |       // read measurements at this timestamp
103 |       meas_package.sensor_type_ = MeasurementPackage::LASER;
104 |       meas_package.raw_measurements_ = VectorXd(2);
105 |       float px;
106 |       float py;
107 |       iss >> px;
108 |       iss >> py;
109 |       meas_package.raw_measurements_ << px, py;
110 |       iss >> timestamp;
111 |       meas_package.timestamp_ = timestamp;
112 |       measurement_pack_list.push_back(meas_package);
113 |     } else if (sensor_type.compare("R") == 0) {
114 |       // radar measurement
115 | 
116 |       // read measurements at this timestamp
117 |       meas_package.sensor_type_ = MeasurementPackage::RADAR;
118 |       meas_package.raw_measurements_ = VectorXd(3);
119 |       float ro;
120 |       float phi;
121 |       float ro_dot;
122 |       iss >> ro;
123 |       iss >> phi;
124 |       iss >> ro_dot;
125 |       meas_package.raw_measurements_ << ro, phi, ro_dot;
126 |       iss >> timestamp;
127 |       meas_package.timestamp_ = timestamp;
128 |       measurement_pack_list.push_back(meas_package);
129 |     }
130 | 
131 |     // read ground truth data to compare later
132 |     float x_gt;
133 |     float y_gt;
134 |     float vx_gt;
135 |     float vy_gt;
136 |     iss >> x_gt;
137 |     iss >> y_gt;
138 |     iss >> vx_gt;
139 |     iss >> vy_gt;
140 |     gt_package.gt_values_ = VectorXd(4);
141 |     gt_package.gt_values_ << x_gt, y_gt, vx_gt, vy_gt;
142 |     gt_pack_list.push_back(gt_package);
143 |   }
144 | 
145 |   // 初始化UKF
146 |   UKF ukf;
147 | 
148 |   // used to compute the RMSE later
149 |   vector<VectorXd> estimations;
150 |   vector<VectorXd> ground_truth;
151 | 
152 |   size_t number_of_measurements = measurement_pack_list.size();
153 | 
154 |   // column names for output file
155 |   out_file_ << "px" << "\t";
156 |   out_file_ << "py" << "\t";
157 |   out_file_ << "v" << "\t";
158 |   out_file_ << "yaw_angle" << "\t";
159 |   out_file_ << "yaw_rate" << "\t";
160 |   out_file_ << "px_measured" << "\t";
161 |   out_file_ << "py_measured" << "\t";
162 |   out_file_ << "px_true" << "\t";
163 |   out_file_ << "py_true" << "\t";
164 |   out_file_ << "vx_true" << "\t";
165 |   out_file_ << "vy_true" << "\t";
166 |   out_file_ << "NIS" << "\n";
167 | 
168 |   for (size_t k = 0; k < number_of_measurements; ++k) {
169 |     // Call the UKF-based fusion
170 |     ukf.ProcessMeasurement(measurement_pack_list[k]);
171 | 
172 |     // output the estimation
173 |     out_file_ << ukf.x_(0) << "\t"; // pos1 - est
174 |     out_file_ << ukf.x_(1) << "\t"; // pos2 - est
175 |     out_file_ << ukf.x_(2) << "\t"; // vel_abs -est
176 |     out_file_ << ukf.x_(3) << "\t"; // yaw_angle -est
177 |     out_file_ << ukf.x_(4) << "\t"; // yaw_rate -est
178 | 
179 |     // output the measurements
180 |     if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::LASER) {
181 |       // output the estimation
182 | 
183 |       // p1 - meas
184 |       out_file_ << measurement_pack_list[k].raw_measurements_(0) << "\t";
185 | 
186 |       // p2 - meas
187 |       out_file_ << measurement_pack_list[k].raw_measurements_(1) << "\t";
188 |     } else if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::RADAR) {
189 |       // output the estimation in the cartesian coordinates
190 |       float ro = measurement_pack_list[k].raw_measurements_(0);
191 |       float phi = measurement_pack_list[k].raw_measurements_(1);
192 |       out_file_ << ro * cos(phi) << "\t"; // p1_meas
193 |       out_file_ << ro * sin(phi) << "\t"; // p2_meas
194 |     }
195 | 
196 |     // output the ground truth packages
197 |     out_file_ << gt_pack_list[k].gt_values_(0) << "\t";
198 |     out_file_ << gt_pack_list[k].gt_values_(1) << "\t";
199 |     out_file_ << gt_pack_list[k].gt_values_(2) << "\t";
200 |     out_file_ << gt_pack_list[k].gt_values_(3) << "\t";
201 | 
202 |     // output the NIS values
203 | 
204 |     if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::LASER) {
205 |       out_file_ << ukf.NIS_laser_ << "\n";
206 |     } else if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::RADAR) {
207 |       out_file_ << ukf.NIS_radar_ << "\n";
208 |     }
209 | 
210 | 
211 |     // convert ukf x vector to cartesian to compare to ground truth
212 |     VectorXd ukf_x_cartesian_ = VectorXd(4);
213 | 
214 |     float x_estimate_ = ukf.x_(0);
215 |     float y_estimate_ = ukf.x_(1);
216 |     float vx_estimate_ = ukf.x_(2) * cos(ukf.x_(3));
217 |     float vy_estimate_ = ukf.x_(2) * sin(ukf.x_(3));
218 | 
219 |     ukf_x_cartesian_ << x_estimate_, y_estimate_, vx_estimate_, vy_estimate_;
220 | 
221 |     estimations.push_back(ukf_x_cartesian_);
222 |     ground_truth.push_back(gt_pack_list[k].gt_values_);
223 | 
224 |   }
225 | 
226 |   // compute the accuracy (RMSE)
227 |   Tools tools;
228 |   cout << "Accuracy - RMSE:" << endl << tools.CalculateRMSE(estimations, ground_truth) << endl;
229 | 
230 |   // close files
231 |   if (out_file_.is_open()) {
232 |     out_file_.close();
233 |   }
234 | 
235 |   if (in_file_.is_open()) {
236 |     in_file_.close();
237 |   }
238 | 
239 |   cout << "Done!" << endl;
240 |   return 0;
241 | }


--------------------------------------------------------------------------------
/src/fusion_ukf.cpp:
--------------------------------------------------------------------------------
  1 | #include "kalman_filter/fusion_ukf.h"
  2 | #include "kalman_filter/tools.h"
  3 | #include "Eigen/Dense"
  4 | #include <iostream>
  5 | 
  6 | #define EPS 0.001 // Just a small number
  7 | 
  8 | using namespace std;
  9 | using Eigen::MatrixXd;
 10 | using Eigen::VectorXd;
 11 | using std::vector;
 12 | 
 13 | /**
 14 |  * Initializes Unscented Kalman filter
 15 |  */
 16 | UKF::UKF() {
 17 |   is_initialized_ = false;
 18 | 
 19 |   // 是否融合这些观测数据
 20 |   use_laser_ = true;
 21 |   use_radar_ = true;
 22 | 
 23 |   // 系统的状态量和不确定度
 24 |   x_ = VectorXd(5);
 25 |   P_ = MatrixXd(5, 5);
 26 | 
 27 |   // 纵向加速度上的过程噪声标准差  m/s^2
 28 |   std_a_ = 1.5;
 29 |   //  yaw加速度上的过程噪声标准差 in rad/s^2
 30 |   std_yawdd_ = 0.57;
 31 |   // 激光雷达在px上测测量噪声标准差 m
 32 |   std_laspx_ = 0.15;
 33 |   // 激光雷达在py上测测量噪声标准差 m
 34 |   std_laspy_ = 0.15;
 35 |   // 毫米波测量的rho标准差 m
 36 |   std_radr_ = 0.3;
 37 |   // 毫米波测量的phi角度标准差
 38 |   std_radphi_ = 0.03;
 39 |   // 毫米波测量的角度变化率rho_phi in m/s
 40 |   std_radrd_ = 0.3;
 41 | 
 42 |   n_x_ = x_.size();   // 系统状态维度
 43 |   n_aug_ = n_x_ + 2; // 采样点的状态维度, 这里直接7个维度
 44 |   n_sig_ = 2 * n_aug_ + 1; // sigma点个数, 15个
 45 | 
 46 |   Xsig_pred_ = MatrixXd(n_x_, n_sig_);  // 预测的sigma点集状态矩阵 5*15
 47 |   lambda_ = 3 - n_aug_;  // sigma点扩散参数
 48 |   weights_ = VectorXd(n_sig_); // sigma点权重
 49 | 
 50 |   // 初始化测量毫米波和激光的噪声矩阵R
 51 |   R_radar_ = MatrixXd(3, 3);
 52 |   R_radar_ << std_radr_ * std_radr_, 0, 0,
 53 |       0, std_radphi_ * std_radphi_, 0,
 54 |       0, 0, std_radrd_ * std_radrd_;
 55 | 
 56 |   R_lidar_ = MatrixXd(2, 2);
 57 |   R_lidar_ << std_laspx_ * std_laspx_, 0,
 58 |       0, std_laspy_ * std_laspy_;
 59 | }
 60 | 
 61 | UKF::~UKF() {}
 62 | 
 63 | /**
 64 |  *  Angle normalization to [-Pi, Pi]
 65 |  */
 66 | void UKF::NormAng(double *ang) {
 67 |   while (*ang > M_PI) *ang -= 2. * M_PI;
 68 |   while (*ang < -M_PI) *ang += 2. * M_PI;
 69 | }
 70 | 
 71 | /**
 72 |  * @param {MeasurementPackage} meas_package The latest measurement data of
 73 |  * either radar or laser.
 74 |  */
 75 | void UKF::ProcessMeasurement(MeasurementPackage measurement_pack) {
 76 |   // CTRV Model, x_ is [px, py, vel, ang, ang_rate]
 77 |   if (!is_initialized_) {
 78 |     // 初始化系统协方差矩阵P
 79 |     P_ << 1, 0, 0, 0, 0,
 80 |         0, 1, 0, 0, 0,
 81 |         0, 0, 1, 0, 0,
 82 |         0, 0, 0, 1, 0,
 83 |         0, 0, 0, 0, 1;
 84 |     if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
 85 |       // 毫米波雷达数据的话
 86 |       float rho = measurement_pack.raw_measurements_[0]; // range
 87 |       float phi = measurement_pack.raw_measurements_[1]; // bearing
 88 |       float rho_dot = measurement_pack.raw_measurements_[2]; // velocity of rho
 89 |       // 求系统状态并作为初值
 90 |       float px = rho * cos(phi);
 91 |       float py = rho * sin(phi);
 92 |       float vx = rho_dot * cos(phi);
 93 |       float vy = rho_dot * sin(phi);
 94 |       float v = sqrt(vx * vx + vy * vy);
 95 |       x_ << px, py, v, 0, 0; // 另外两个数据没有，设为0
 96 |     } else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
 97 |       // 激光雷达数据，只有位置px,py, 其他初始化为0
 98 |       x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0, 0;
 99 |       // Deal with the special case initialisation problems
100 |       if (fabs(x_(0)) < EPS and fabs(x_(1)) < EPS) {
101 |         x_(0) = EPS;
102 |         x_(1) = EPS;
103 |       }
104 |     }
105 | 
106 |     // 初始化sigma点权重,一共15个sigma点
107 |     weights_(0) = lambda_ / (lambda_ + n_aug_);  // 第一个点的权重lamda/(n+lamda)
108 |     for (int i = 1; i < weights_.size(); i++) {  // 之后每个点的权重 1/2(n+lamda)
109 |       weights_(i) = 0.5 / (n_aug_ + lambda_);
110 |     }
111 | 
112 |     time_us_ = measurement_pack.timestamp_;  // 更新时间戳
113 |     is_initialized_ = true;
114 |     //cout << "Init" << endl;
115 |     //cout << "x_" << x_ << endl;
116 |     return;
117 |   }
118 | 
119 |   // 计算delta_t并更新
120 |   double dt = (measurement_pack.timestamp_ - time_us_);
121 |   dt /= 1000000.0; // convert micros to s
122 |   time_us_ = measurement_pack.timestamp_;
123 | 
124 |   // 根据上一帧状态，预测当前状态，计算转换后的sigma点，并拟合高斯
125 |   Prediction(dt);
126 |   //cout << "predict:" << endl;
127 |   //cout << "x_" << endl << x_ << endl;
128 | 
129 |   // 根据传感器数据类型去获取融合结果
130 |   if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR && use_radar_) {
131 |     //cout << "Radar " << measurement_pack.raw_measurements_[0] << " " << measurement_pack.raw_measurements_[1] << endl;
132 |     UpdateRadar(measurement_pack);
133 |   }
134 |   if (measurement_pack.sensor_type_ == MeasurementPackage::LASER && use_laser_) {
135 |     //cout << "Lidar " << measurement_pack.raw_measurements_[0] << " " << measurement_pack.raw_measurements_[1] << endl;
136 |     UpdateLidar(measurement_pack);
137 |   }
138 | }
139 | 
140 | /**
141 |  * Predicts sigma points, the state, and the state covariance matrix.
142 |  * @param {double} delta_t the change in time (in seconds) between the last
143 |  * measurement and this one.
144 |  */
145 | void UKF::Prediction(double delta_t) {
146 |   // 匀加速运动模型，这里是dt^2
147 |   double delta_t2 = delta_t * delta_t;
148 | 
149 |   // 扩展到7维的状态
150 |   VectorXd x_aug = VectorXd(n_aug_);    // Augmented mean vector 7×1
151 |   MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);    // Augmented state covarience matrix 7×7
152 |   MatrixXd Xsig_aug = MatrixXd(n_aug_, n_sig_);    // Sigma point matrix 7*15
153 | 
154 |   // Fill the matrices
155 |   x_aug.fill(0.0);
156 |   x_aug.head(n_x_) = x_;
157 |   P_aug.fill(0);
158 |   P_aug.topLeftCorner(n_x_, n_x_) = P_;
159 | 
160 |   P_aug(5, 5) = std_a_ * std_a_;  // 剩下两个维度对加速度和yaw变化率噪声进行估计
161 |   P_aug(6, 6) = std_yawdd_ * std_yawdd_;
162 |   MatrixXd L = P_aug.llt().matrixL();  // Square root of P matrix
163 | 
164 |   // 创建sigma点 7*15
165 |   Xsig_aug.col(0) = x_aug;
166 |   double sqrt_lambda_n_aug = sqrt(lambda_ + n_aug_); // Save some computations
167 |   VectorXd sqrt_lambda_n_aug_L;
168 |   for (int i = 0; i < n_aug_; i++) {
169 |     // 每一行赋值
170 |     sqrt_lambda_n_aug_L = sqrt_lambda_n_aug * L.col(i);
171 |     Xsig_aug.col(i + 1) = x_aug + sqrt_lambda_n_aug_L;
172 |     Xsig_aug.col(i + 1 + n_aug_) = x_aug - sqrt_lambda_n_aug_L;
173 |   }
174 | 
175 |   // 预测sigma点的状态，根据匀加速运动模型来计算
176 |   for (int i = 0; i < n_sig_; i++) {
177 |     // Extract values for better readability
178 |     double p_x = Xsig_aug(0, i);
179 |     double p_y = Xsig_aug(1, i);
180 |     double v = Xsig_aug(2, i);
181 |     double yaw = Xsig_aug(3, i);
182 |     double yawd = Xsig_aug(4, i);
183 |     double nu_a = Xsig_aug(5, i);
184 |     double nu_yawdd = Xsig_aug(6, i);
185 |     // Precalculate sin and cos for optimization
186 |     double sin_yaw = sin(yaw);
187 |     double cos_yaw = cos(yaw);
188 |     double arg = yaw + yawd * delta_t;
189 | 
190 |     // Predicted state values
191 |     double px_p, py_p;
192 |     // Avoid division by zero
193 |     if (fabs(yawd) > EPS) {
194 |       double v_yawd = v / yawd;
195 |       px_p = p_x + v_yawd * (sin(arg) - sin_yaw);
196 |       py_p = p_y + v_yawd * (cos_yaw - cos(arg));
197 |     } else {
198 |       double v_delta_t = v * delta_t;
199 |       px_p = p_x + v_delta_t * cos_yaw;
200 |       py_p = p_y + v_delta_t * sin_yaw;
201 |     }
202 |     double v_p = v;
203 |     double yaw_p = arg;
204 |     double yawd_p = yawd;
205 | 
206 |     // Add noise
207 |     px_p += 0.5 * nu_a * delta_t2 * cos_yaw;
208 |     py_p += 0.5 * nu_a * delta_t2 * sin_yaw;
209 |     v_p += nu_a * delta_t;
210 |     yaw_p += 0.5 * nu_yawdd * delta_t2;
211 |     yawd_p += nu_yawdd * delta_t;
212 | 
213 |     // Write predicted sigma point into right column
214 |     Xsig_pred_(0, i) = px_p;
215 |     Xsig_pred_(1, i) = py_p;
216 |     Xsig_pred_(2, i) = v_p;
217 |     Xsig_pred_(3, i) = yaw_p;
218 |     Xsig_pred_(4, i) = yawd_p;
219 |   }
220 | 
221 |   // 计算系统的加权均值以及不确定度
222 |   x_ = Xsig_pred_ * weights_; // vectorised sum
223 |   // Predicted state covariance matrix
224 |   P_.fill(0.0);
225 |   for (int i = 0; i < n_sig_; i++) {  //iterate over sigma points
226 |     // State difference
227 |     VectorXd x_diff = Xsig_pred_.col(i) - x_;
228 |     // Angle normalization
229 |     NormAng(&(x_diff(3)));
230 |     P_ = P_ + weights_(i) * x_diff * x_diff.transpose();
231 |   }
232 | }
233 | 
234 | /**
235 |  * Updates the state and the state covariance matrix using a radar measurement.
236 |  * @param {MeasurementPackage} meas_package
237 |  */
238 | void UKF::UpdateRadar(MeasurementPackage meas_package) {
239 |   int n_z = 3;   // 观测变量只有3维
240 |   MatrixXd Zsig = MatrixXd(n_z, n_sig_);
241 |   // 将预测的7维状态变量的sigma点转换到3维观测空间
242 |   for (int i = 0; i < n_sig_; i++) {
243 |     // 列为状态空间维度，行为每一个sigma点
244 |     double p_x = Xsig_pred_(0, i);
245 |     double p_y = Xsig_pred_(1, i);
246 |     double v = Xsig_pred_(2, i);
247 |     double yaw = Xsig_pred_(3, i);
248 |     double v1 = cos(yaw) * v;
249 |     double v2 = sin(yaw) * v;
250 |     // Measurement model
251 |     Zsig(0, i) = sqrt(p_x * p_x + p_y * p_y);          //r
252 |     Zsig(1, i) = atan2(p_y, p_x);                   //phi
253 |     Zsig(2, i) = (p_x * v1 + p_y * v2) / Zsig(0, i);   //r_dot
254 |   }
255 |   UpdateUKF(meas_package, Zsig, n_z);
256 | }
257 | 
258 | /**
259 |  * Updates the state and the state covariance matrix using a laser measurement.
260 |  * @param {MeasurementPackage} meas_package
261 |  */
262 | void UKF::UpdateLidar(MeasurementPackage meas_package) {
263 |   int n_z = 2;  //  设置观测维度
264 |   // 将7*15的sigma 点集合转换到测量空间
265 |   MatrixXd Zsig = Xsig_pred_.block(0, 0, n_z, n_sig_);
266 |   UpdateUKF(meas_package, Zsig, n_z);
267 | }
268 | 
269 | // Universal update function
270 | void UKF::UpdateUKF(MeasurementPackage meas_package, MatrixXd Zsig, int n_z) {
271 | 
272 |   VectorXd z_pred = VectorXd(n_z); // 观测状态量 3*1或者2*1
273 |   z_pred = Zsig * weights_;   // sigma点集加权均值
274 |   MatrixXd S = MatrixXd(n_z, n_z);  //  观测不确定度 3*3或者2*2
275 |   S.fill(0.0);
276 | 
277 |   // 计算状态量的加权均值
278 |   for (int i = 0; i < n_sig_; i++) {
279 |     // Residual
280 |     VectorXd z_diff = Zsig.col(i) - z_pred;  // 计算每个点状态量的残差
281 |     NormAng(&(z_diff(1)));
282 |     S = S + weights_(i) * z_diff * z_diff.transpose();
283 |   }
284 | 
285 |   // 测量噪声也加入进来
286 |   MatrixXd R = MatrixXd(n_z, n_z);
287 |   if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { // Radar
288 |     R = R_radar_;
289 |   } else if (meas_package.sensor_type_ == MeasurementPackage::LASER) { // Lidar
290 |     R = R_lidar_;
291 |   }
292 |   S = S + R;
293 | 
294 |   // 计算关联矩阵T 5*3或者5*2
295 |   MatrixXd Tc = MatrixXd(n_x_, n_z);
296 |   Tc.fill(0.0);
297 |   for (int i = 0; i < n_sig_; i++) {
298 |     //residual
299 |     VectorXd z_diff = Zsig.col(i) - z_pred;
300 |     if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { // Radar
301 |       NormAng(&(z_diff(1)));        // Angle normalization
302 |     }
303 |     // State difference
304 |     VectorXd x_diff = Xsig_pred_.col(i) - x_;
305 |     // Angle normalization
306 |     NormAng(&(x_diff(3)));
307 |     Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
308 |   }
309 |   // Measurements
310 |   VectorXd z = meas_package.raw_measurements_;
311 |   //Kalman gain K;
312 |   MatrixXd K = Tc * S.inverse();
313 |   // Residual
314 |   VectorXd z_diff = z - z_pred;
315 |   if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { // Radar
316 |     // Angle normalization
317 |     NormAng(&(z_diff(1)));
318 |   }
319 |   // Update state mean and covariance matrix
320 |   x_ = x_ + K * z_diff;
321 |   P_ = P_ - K * S * K.transpose();
322 |   // Calculate NIS
323 |   if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { // Radar
324 |     NIS_radar_ = z.transpose() * S.inverse() * z;
325 |   } else if (meas_package.sensor_type_ == MeasurementPackage::LASER) { // Lidar
326 |     NIS_laser_ = z.transpose() * S.inverse() * z;
327 |   }
328 | }
329 | 


--------------------------------------------------------------------------------
/results/output2.txt:
--------------------------------------------------------------------------------
  1 | 0.0001	0.0001	0	0	0	0	0	0	0	0
  2 | 0.000338488	-0.000446911	0	0	0	0	0	0	0	0
  3 | 1.55415	-0.143754	2.01017	0.309235	1.55945	-0.138501	2.09897	0.0522228	2.19595	0.109339
  4 | 1.62109	0.0192285	2.30043	0.494589	1.81209	0.0474826	2.09897	0.0522228	2.19595	0.109339
  5 | 3.89102	-0.130397	2.26271	-0.525834	3.89093	-0.134166	4.29136	0.215312	2.28434	0.226323
  6 | 3.96437	0.0739224	2.42288	-0.197898	4.19528	0.218461	4.29136	0.215312	2.28434	0.226323
  7 | 6.85906	0.413143	3.33637	0.702939	6.86352	0.416817	6.56942	0.496096	2.36382	0.348847
  8 | 6.6903	0.432431	2.62654	0.574797	6.43926	0.472879	6.56942	0.496096	2.36382	0.348847
  9 | 9.07965	0.596255	2.15901	-0.135533	9.07733	0.593211	8.92437	0.89924	2.43359	0.474526
 10 | 9.09362	0.667446	2.40714	0.00382532	8.89749	0.886982	8.92437	0.89924	2.43359	0.474526
 11 | 11.5747	1.65959	2.56527	1.70838	11.5756	1.6669	11.3468	1.427	2.49328	0.600781
 12 | 11.5246	1.61821	2.36441	1.59307	11.4447	1.42958	11.3468	1.427	2.49328	0.600781
 13 | 13.5952	2.31862	1.76386	0.0442223	13.5921	2.31191	13.827	2.07905	2.5429	0.724947
 14 | 13.6906	2.29565	2.32833	0.0971446	13.6572	1.99915	13.827	2.07905	2.5429	0.724947
 15 | 16.6395	2.89904	3.55283	0.991102	16.6456	2.903	16.3554	2.85243	2.58284	0.844365
 16 | 16.4806	2.87106	2.85466	0.861002	16.282	2.84648	16.3554	2.85243	2.58284	0.844365
 17 | 19.0137	3.70587	2.22932	0.802891	19.0106	3.70555	18.923	3.74161	2.61382	0.95648
 18 | 19.0872	3.72615	2.38026	0.842603	19.363	3.87122	18.923	3.74161	2.61382	0.95648
 19 | 21.3324	4.73093	2.12594	1.11391	21.3312	4.73205	21.5215	4.73855	2.63679	1.05891
 20 | 21.4286	4.75053	2.48866	1.20798	21.6213	4.74762	21.5215	4.73855	2.63679	1.05891
 21 | 24.0774	5.57241	2.77833	0.555692	24.0787	5.56967	24.1437	5.83295	2.65286	1.14953
 22 | 24.1858	5.60662	3.0073	0.624424	24.5601	5.92334	24.1437	5.83295	2.65286	1.14953
 23 | 26.6104	6.95605	1.91948	1.83217	26.6052	6.96102	26.7833	7.01246	2.6632	1.22647
 24 | 26.669	6.97214	2.35329	1.98497	26.5419	6.90075	26.7833	7.01246	2.6632	1.22647
 25 | 29.5816	8.09386	3.38012	0.541373	29.5864	8.08789	29.4352	8.26299	2.66893	1.28822
 26 | 29.5271	8.08402	3.09614	0.466634	29.4716	8.24519	29.4352	8.26299	2.66893	1.28822
 27 | 31.9471	9.87095	1.8757	2.6839	31.9416	9.88017	32.0955	9.56901	2.67105	1.33358
 28 | 31.9327	9.86053	1.98511	2.72836	31.7928	9.49891	32.0955	9.56901	2.67105	1.33358
 29 | 34.6105	10.7121	3.18342	-0.443884	34.6157	10.6988	34.7608	10.9139	2.67035	1.3617
 30 | 34.6942	10.741	3.28977	-0.4049	35.0599	10.9953	34.7608	10.9139	2.67035	1.3617
 31 | 37.147	12.0761	1.802	2.52021	37.1404	12.0883	37.4287	12.2803	2.66736	1.37209
 32 | 37.2622	12.1165	2.20137	2.66782	37.4973	12.2943	37.4287	12.2803	2.66736	1.37209
 33 | 40.3215	13.4756	3.75256	0.484602	40.3285	13.4666	40.0971	13.6505	2.66232	1.36459
 34 | 40.1874	13.4356	3.1988	0.302444	39.9009	13.6243	40.0971	13.6505	2.66232	1.36459
 35 | 42.7713	14.8218	2.09976	2.1173	42.7664	14.8294	42.7638	15.0065	2.65515	1.33938
 36 | 42.7953	14.8338	2.36692	2.22852	42.5714	14.9817	42.7638	15.0065	2.65515	1.33938
 37 | 45.3558	16.556	2.69835	1.37297	45.3572	16.5524	45.4265	16.3307	2.64546	1.29701
 38 | 45.4025	16.5716	2.77683	1.3981	45.649	16.5213	45.4265	16.3307	2.64546	1.29701
 39 | 48.1549	17.2885	2.6736	0.223198	48.1541	17.2835	48.0823	17.6063	2.6326	1.23832
 40 | 48.1527	17.2906	2.76745	0.259331	47.9331	17.5349	48.0823	17.6063	2.6326	1.23832
 41 | 50.6243	19.0612	2.32289	2.83589	50.6228	19.0721	50.7276	18.8172	2.61569	1.16454
 42 | 50.5741	19.0396	2.12039	2.74739	50.5737	18.7834	50.7276	18.8172	2.61569	1.16454
 43 | 53.7188	19.9113	3.9236	-0.398974	53.7268	19.8982	53.3577	19.9486	2.59372	1.07716
 44 | 53.6036	19.8704	3.3505	-0.596477	53.5277	19.862	53.3577	19.9486	2.59372	1.07716
 45 | 56.0393	21.0797	1.74676	2.4372	56.0322	21.0923	55.9669	20.9874	2.56561	0.978022
 46 | 56.0303	21.0766	1.84832	2.47996	55.867	20.985	55.9669	20.9874	2.56561	0.978022
 47 | 58.6574	21.7945	3.19094	-0.490703	58.6632	21.7821	58.5484	21.9225	2.53033	0.869205
 48 | 58.6557	21.7952	3.1878	-0.489578	58.5799	21.9815	58.5484	21.9225	2.53033	0.869205
 49 | 60.88	22.7775	1.46369	1.96475	60.8722	22.7876	61.0941	22.7449	2.48697	0.753031
 50 | 60.9212	22.7934	1.85305	2.11731	60.7648	22.6398	61.0941	22.7449	2.48697	0.753031
 51 | 63.3777	23.4175	2.88317	-0.404352	63.3821	23.4069	63.5955	23.4479	2.43482	0.631987
 52 | 63.438	23.4411	2.88902	-0.396882	63.7478	23.7442	63.5955	23.4479	2.43482	0.631987
 53 | 66.3394	23.9889	2.99152	1.23058	66.3403	23.9959	66.0431	24.0276	2.37349	0.508655
 54 | 66.1698	23.928	2.27246	0.979948	65.8838	23.9541	66.0431	24.0276	2.37349	0.508655
 55 | 68.2641	24.3623	1.88592	0.0264649	68.262	24.3582	68.4273	24.4824	2.30293	0.385623
 56 | 68.4083	24.4139	2.41734	0.211611	68.6759	24.6059	68.4273	24.4824	2.30293	0.385623
 57 | 70.8273	24.8825	2.44139	0.653677	70.8275	24.8844	70.7384	24.8137	2.22347	0.265395
 58 | 70.7719	24.8623	2.21238	0.574144	70.7403	24.6728	70.7384	24.8137	2.22347	0.265395
 59 | 72.8937	24.7189	1.98072	-0.666701	72.8923	24.7135	72.9675	25.025	2.13584	0.150292
 60 | 72.9792	24.7486	2.30324	-0.563106	73.0732	25.0343	72.9675	25.025	2.13584	0.150292
 61 | 75.0039	25.2518	1.85107	1.2463	75.0021	25.2595	75.1062	25.1224	2.04114	0.0423626
 62 | 74.9802	25.2434	1.75283	1.21262	74.9748	25.1377	75.1062	25.1224	2.04114	0.0423626
 63 | 77.12	24.9405	2.3784	-1.35949	77.1225	24.9297	77.1475	25.1137	1.9408	-0.0566978
 64 | 77.1134	24.9402	2.23022	-1.39709	77.1386	25.2948	77.1475	25.1137	1.9408	-0.0566978
 65 | 79.11	25.0262	1.89208	1.12899	79.1089	25.0369	79.0861	25.0086	1.83649	-0.145607
 66 | 79.0734	25.0141	1.62212	1.04643	79.1718	24.9698	79.0861	25.0086	1.83649	-0.145607
 67 | 80.8072	24.8774	1.74951	-0.9735	80.8074	24.8688	80.9183	24.8178	1.73008	-0.223536
 68 | 80.8647	24.8947	1.84799	-0.943153	81.1423	24.8704	80.9183	24.8178	1.73008	-0.223536
 69 | 82.622	24.6344	1.72251	0.219904	82.6216	24.6393	82.6423	24.5525	1.6235	-0.29012
 70 | 82.5827	24.6234	1.52136	0.165049	82.5085	24.8114	82.6423	24.5525	1.6235	-0.29012
 71 | 84.1729	24.5521	1.63723	-0.234727	84.1733	24.5504	84.2584	24.2245	1.51869	-0.345444
 72 | 84.1797	24.5533	1.63712	-0.23578	84.2898	24.3312	84.2584	24.2245	1.51869	-0.345444
 73 | 85.6398	23.8118	1.26079	-1.11328	85.6378	23.808	85.7688	23.8451	1.41749	-0.390006
 74 | 85.7256	23.8349	1.54821	-1.04032	85.9743	23.7547	85.7688	23.8451	1.41749	-0.390006
 75 | 86.9475	23.522	0.966863	0.181192	86.9449	23.527	87.1776	23.4252	1.32159	-0.424654
 76 | 87.0599	23.5518	1.13591	0.227366	87.6171	23.5127	87.1776	23.4252	1.32159	-0.424654
 77 | 88.8174	22.7937	2.27202	-1.41572	88.8227	22.7869	88.4904	22.9745	1.23248	-0.450514
 78 | 88.6883	22.7621	1.74243	-1.5345	88.4191	22.9498	88.4904	22.9745	1.23248	-0.450514
 79 | 89.5048	22.3009	0.0195024	0.240042	89.4967	22.3082	89.7145	22.5017	1.15138	-0.468904
 80 | 89.634	22.3336	0.577943	0.3734	89.7821	22.4826	89.7145	22.5017	1.15138	-0.468904
 81 | 90.511	21.675	1.09378	-1.36904	90.5133	21.6677	90.858	22.0141	1.07929	-0.481241
 82 | 90.6023	21.6977	1.31756	-1.31553	90.8419	22.0974	90.858	22.0141	1.07929	-0.481241
 83 | 91.7193	21.1101	0.971842	-0.086387	91.7178	21.1152	91.9302	21.5174	1.01698	-0.488964
 84 | 91.7765	21.1242	0.980908	-0.0793717	92.0492	21.5289	91.9302	21.5174	1.01698	-0.488964
 85 | 92.859	20.9543	1.1723	-0.227902	92.8599	20.9537	92.9411	21.016	0.964993	-0.493443
 86 | 92.9079	20.9655	1.07757	-0.245694	93.3186	21.0712	92.9411	21.016	0.964993	-0.493443
 87 | 93.472	20.2194	0.0560218	-1.1264	93.4668	20.2155	93.9011	20.513	0.92369	-0.495919
 88 | 93.6323	20.254	0.800877	-0.98001	93.7423	20.5071	93.9011	20.513	0.92369	-0.495919
 89 | 95.3143	19.6553	2.52761	-0.284208	95.3229	19.6585	94.8209	20.01	0.893308	-0.497439
 90 | 95.0094	19.5941	1.23197	-0.524167	94.494	19.8854	94.8209	20.01	0.893308	-0.497439
 91 | 95.5105	19.6943	-0.152303	0.500752	95.5039	19.6984	95.7116	19.5078	0.87398	-0.498816
 92 | 95.6121	19.7152	0.430349	0.621647	95.6097	19.5804	95.7116	19.5078	0.87398	-0.498816
 93 | 96.2625	19.168	0.797252	-1.35544	96.2641	19.1597	96.5842	19.0065	0.865773	-0.500589
 94 | 96.3223	19.1789	0.97708	-1.32316	96.559	18.8688	96.5842	19.0065	0.865773	-0.500589
 95 | 97.5261	18.7206	1.45885	0.158666	97.5286	18.727	97.45	18.506	0.868716	-0.503009
 96 | 97.4439	18.7035	1.01503	0.077536	97.5333	18.1735	97.45	18.506	0.868716	-0.503009
 97 | 98.213	18.3769	0.518857	-0.621104	98.2105	18.3739	98.3198	18.0049	0.882805	-0.506023
 98 | 98.2756	18.3879	0.779833	-0.57614	98.4162	18.1556	98.3198	18.0049	0.882805	-0.506023
 99 | 99.0367	17.3602	0.722475	-1.34425	99.0363	17.357	99.2049	17.503	0.908006	-0.509278
100 | 99.0751	17.3664	1.02804	-1.30161	98.9782	17.2949	99.2049	17.503	0.908006	-0.509278
101 | 99.9975	16.9817	0.863034	0.250894	99.9969	16.9882	100.117	17.0001	0.944242	-0.512134
102 | 100.056	16.9914	0.862789	0.252413	100.441	16.9521	100.117	17.0001	0.944242	-0.512134
103 | 101.201	16.5418	1.38105	-0.927518	101.204	16.5368	101.065	16.497	0.991362	-0.513687
104 | 101.157	16.5345	1.13968	-0.963097	101.169	16.4353	101.065	16.497	0.991362	-0.513687
105 | 102.386	16.3933	1.34601	0.435614	102.387	16.3992	102.063	15.9949	1.04911	-0.512803
106 | 102.277	16.3746	0.987376	0.377426	102.081	15.8359	102.063	15.9949	1.04911	-0.512803
107 | 103.006	15.3631	0.431207	-1.99018	103.003	15.353	103.119	15.4967	1.11708	-0.508169
108 | 103.065	15.3701	1.03393	-1.92442	102.776	15.1195	103.119	15.4967	1.11708	-0.508169
109 | 104.114	15.2579	1.13046	1.14999	104.114	15.2708	104.244	15.0064	1.19469	-0.498349
110 | 104.057	15.2486	1.01316	1.13102	103.905	14.7607	104.244	15.0064	1.19469	-0.498349
111 | 105.537	14.4548	1.84801	-2.1162	105.541	14.4412	105.447	14.5293	1.28111	-0.481861
112 | 105.451	14.4425	1.54307	-2.15345	105.316	14.0026	105.447	14.5293	1.28111	-0.481861
113 | 106.832	14.0358	1.28789	0.802138	106.831	14.0482	106.737	14.0724	1.37529	-0.457255
114 | 106.776	14.029	1.24487	0.796471	106.439	14.2401	106.737	14.0724	1.37529	-0.457255
115 | 108.083	13.6686	1.3283	-1.16224	108.084	13.6604	108.122	13.644	1.47592	-0.423202
116 | 108.126	13.6738	1.55755	-1.13673	108.131	13.6844	108.122	13.644	1.47592	-0.423202
117 | 109.535	13.5641	1.30224	0.59887	109.534	13.5714	109.606	13.2536	1.58144	-0.378583
118 | 109.514	13.5607	1.33611	0.601308	109.388	13.1556	109.606	13.2536	1.58144	-0.378583
119 | 111.061	12.5925	1.69752	-2.05147	111.063	12.5814	111.196	12.9118	1.69008	-0.322574
120 | 111.143	12.6024	1.86465	-2.03162	111.426	12.9557	111.196	12.9118	1.69008	-0.322574
121 | 112.913	12.4742	1.73515	1.18981	112.913	12.4877	112.893	12.6299	1.7999	-0.254723
122 | 112.89	12.4723	1.65476	1.18219	112.797	12.8241	112.893	12.6299	1.7999	-0.254723
123 | 114.799	12.5583	2.11831	-0.66721	114.801	12.5506	114.699	12.4195	1.90891	-0.175007
124 | 114.788	12.5567	1.96667	-0.683374	114.916	12.3187	114.699	12.4195	1.90891	-0.175007
125 | 116.688	12.1019	1.84354	-0.29812	116.687	12.1035	116.611	12.2921	2.01507	-0.0838683
126 | 116.749	12.1081	2.07684	-0.276336	116.871	12.0659	116.611	12.2921	2.01507	-0.0838683
127 | 118.952	11.9997	2.3262	0.0113285	118.953	12.0009	118.628	12.2586	2.11648	0.0177712
128 | 118.92	11.9973	2.18003	-0.00132765	118.85	12.4063	118.628	12.2586	2.11648	0.0177712
129 | 120.699	12.2898	1.40639	0.485827	120.695	12.2918	120.743	12.3289	2.21139	0.128528
130 | 120.812	12.3012	1.929	0.540118	120.934	12.1905	120.743	12.3289	2.21139	0.128528
131 | 123.56	12.8296	3.52325	0.541531	123.568	12.8297	122.95	12.5114	2.29835	0.246599
132 | 123.356	12.8083	2.59162	0.440987	123.142	12.7694	122.95	12.5114	2.29835	0.246599
133 | 124.904	12.7779	0.548536	-0.384718	124.894	12.7743	125.242	12.8125	2.37624	0.369827
134 | 125.208	12.8089	2.05789	-0.236286	125.376	12.9964	125.242	12.8125	2.37624	0.369827
135 | 127.847	13.3218	3.20799	1.04732	127.853	13.3273	127.609	13.2365	2.44434	0.495787
136 | 127.707	13.3068	2.62711	0.983489	127.506	13.153	127.609	13.2365	2.44434	0.495787
137 | 129.951	13.7006	1.86588	-0.0251618	129.947	13.6963	130.042	13.7853	2.50232	0.621879
138 | 130.072	13.7133	2.29301	0.0204279	130.334	13.781	130.042	13.7853	2.50232	0.621879
139 | 132.094	14.5016	1.7878	1.31347	132.092	14.507	132.53	14.458	2.55028	0.74543
140 | 132.221	14.5156	2.30611	1.37509	132.447	14.3423	132.53	14.458	2.55028	0.74543
141 | 135.069	15.0533	3.33499	-0.0279259	135.074	15.0475	135.066	15.2513	2.58865	0.863795
142 | 134.989	15.0441	2.81698	-0.0876768	135.127	14.6575	135.066	15.2513	2.58865	0.863795
143 | 137.563	16.2059	2.37858	2.02155	137.561	16.2147	137.639	16.1592	2.61821	0.974442
144 | 137.613	16.2124	2.59233	2.05063	137.653	16.5145	137.639	16.1592	2.61821	0.974442
145 | 140.331	17.0169	2.7975	-0.0567298	140.331	17.008	140.242	17.1733	2.63994	1.07503
146 | 140.347	17.0195	2.86758	-0.0468062	140.305	17.5379	140.242	17.1733	2.63994	1.07503
147 | 142.891	17.9847	2.2704	1.65791	142.888	17.9918	142.868	18.2829	2.65497	1.16346
148 | 142.895	17.9857	2.50637	1.69291	142.626	18.1458	142.868	18.2829	2.65497	1.16346
149 | 145.392	19.1653	2.47212	0.82285	145.392	19.1617	145.51	19.4754	2.66445	1.23795
150 | 145.496	19.1792	2.76299	0.863239	145.789	19.3785	145.51	19.4754	2.66445	1.23795
151 | 148.251	20.4723	2.76243	1.5916	148.251	20.4754	148.163	20.7364	2.66951	1.29702
152 | 148.223	20.4689	2.61793	1.57028	148.146	20.9376	148.163	20.7364	2.66951	1.29702
153 | 150.687	22.1137	2.32262	1.69142	150.686	22.1142	150.825	22.0502	2.67111	1.33953
154 | 150.722	22.1187	2.61229	1.74079	150.649	21.8148	150.825	22.0502	2.67111	1.33953
155 | 153.421	23.4934	2.76778	1.12316	153.422	23.4909	153.491	23.3999	2.66999	1.3647
156 | 153.403	23.49	2.68699	1.10874	153.466	22.9115	153.491	23.3999	2.66999	1.3647
157 | 156.047	24.807	2.61185	1.46035	156.047	24.8084	156.159	24.7682	2.66666	1.37208
158 | 156.029	24.8044	2.49616	1.44054	156.009	25.0678	156.159	24.7682	2.66666	1.37208
159 | 158.901	26.0322	3.21775	1.09458	158.905	26.0308	158.827	26.1372	2.66127	1.36157
160 | 158.885	26.0295	2.99321	1.0546	159.014	26.1903	158.827	26.1372	2.66127	1.36157
161 | 161.416	27.4234	2.11036	1.61147	161.412	27.4256	161.493	27.4891	2.65371	1.33344
162 | 161.484	27.4355	2.38818	1.66276	161.529	27.8454	161.493	27.4891	2.65371	1.33344
163 | 163.628	28.7707	1.89893	1.09629	163.625	28.7683	164.155	28.8064	2.64354	1.28825
164 | 163.797	28.801	2.47969	1.20438	164.116	29.315	164.155	28.8064	2.64354	1.28825
165 | 166.823	30.2046	3.54767	1.56655	166.828	30.2062	166.809	30.0721	2.63006	1.22694
166 | 166.69	30.1798	2.84112	1.4289	166.748	29.7121	166.809	30.0721	2.63006	1.22694
167 | 169.101	31.3736	1.99744	1.01011	169.097	31.3717	169.452	31.2706	2.61238	1.15075
168 | 169.189	31.3895	2.45422	1.09854	169.345	30.7063	169.452	31.2706	2.61238	1.15075
169 | 172.17	32.3412	3.4687	0.873502	172.175	32.3404	172.079	32.3873	2.58945	1.06126
170 | 172.091	32.3265	2.94005	0.771587	172.135	32.6666	172.079	32.3873	2.58945	1.06126
171 | 174.646	33.4601	2.21206	1.36879	174.643	33.4625	174.685	33.4095	2.56021	0.960341
172 | 174.655	33.4622	2.33731	1.39513	174.474	33.9157	174.685	33.4095	2.56021	0.960341
173 | 177.03	34.2056	2.37946	0.289327	177.03	34.2009	177.261	34.3262	2.52363	0.850119
174 | 177.06	34.2111	2.47569	0.307569	177.211	33.8467	177.261	34.3262	2.52363	0.850119
175 | 180.139	35.1376	3.67553	1.3884	180.145	35.1423	179.799	35.1289	2.47883	0.732949
176 | 179.907	35.0923	2.64508	1.18316	179.543	35.4767	179.799	35.1289	2.47883	0.732949
177 | 182.599	35.7567	2.7111	0.303646	182.599	35.753	182.292	35.8114	2.42515	0.611338
178 | 182.501	35.738	2.46886	0.256896	182.047	36.2626	182.292	35.8114	2.42515	0.611338
179 | 184.734	36.5119	2.03648	1.12429	184.732	36.5155	184.73	36.37	2.36226	0.487877
180 | 184.736	36.5121	2.02947	1.12235	184.833	36.1451	184.73	36.37	2.36226	0.487877
181 | 186.945	36.9453	2.3426	-0.0404633	186.946	36.9404	187.103	36.804	2.29017	0.365145
182 | 187.01	36.9582	2.46388	-0.01657	187.296	36.9375	187.103	36.804	2.29017	0.365145
183 | 189.465	37.4019	2.46873	0.765419	189.465	37.4052	189.401	37.1147	2.20928	0.245623
184 | 189.454	37.3993	2.24249	0.720097	189.738	36.951	189.401	37.1147	2.20928	0.245623
185 | 191.735	37.567	2.29034	-0.217098	191.735	37.5631	191.615	37.3064	2.12037	0.131591
186 | 191.703	37.5604	2.20377	-0.234389	191.682	37.1147	191.615	37.3064	2.12037	0.131591
187 | 193.475	37.2794	1.36525	-0.334537	193.471	37.2789	193.738	37.3853	2.02462	0.0250452
188 | 193.567	37.2971	1.86005	-0.242849	193.555	37.4234	193.738	37.3853	2.02462	0.0250452
189 | 195.986	37.3964	2.95945	0.365282	195.991	37.3991	195.762	37.3598	1.92349	-0.072386
190 | 195.785	37.3582	2.07145	0.199258	195.507	37.4891	195.762	37.3598	1.92349	-0.072386
191 | 197.726	37.7274	1.82572	0.481806	197.725	37.7285	197.683	37.2395	1.81869	-0.159495
192 | 197.691	37.7205	1.79369	0.475296	197.582	37.303	197.683	37.2395	1.81869	-0.159495
193 | 199.296	37.2927	1.38567	-1.06834	199.294	37.2862	199.497	37.0353	1.71212	-0.235529
194 | 199.353	37.3033	1.68851	-1.01466	199.356	37.3527	199.497	37.0353	1.71212	-0.235529
195 | 201.544	36.9921	2.69514	0.203906	201.549	36.9973	201.203	36.7587	1.6057	-0.300201
196 | 201.335	36.954	1.80514	0.0468283	200.932	37.4694	201.203	36.7587	1.6057	-0.300201
197 | 202.964	36.4608	1.44018	-0.878651	202.962	36.4568	202.801	36.4212	1.50137	-0.353671
198 | 202.938	36.4566	1.42104	-0.881796	202.65	37.2445	202.801	36.4212	1.50137	-0.353671
199 | 203.994	36.1924	0.739444	0.150829	203.991	36.1967	204.294	36.0343	1.40095	-0.396497
200 | 204.044	36.2015	1.20283	0.230669	203.809	36.2941	204.294	36.0343	1.40095	-0.396497
201 | 


--------------------------------------------------------------------------------
/data/sample-laser-radar-measurement-data-2.txt:
--------------------------------------------------------------------------------
  1 | L	0	0	1477010443349642	0	0	0	0
  2 | R	0	0	0	1477010443349642	0	0	0	0
  3 | L	1.559445e+00	-1.385015e-01	1477010444349642	2.098967e+00	5.222280e-02	2.195949e+00	1.093391e-01
  4 | R	1.812711e+00	2.619727e-02	2.305732e+00	1477010444349642	2.098967e+00	5.222280e-02	2.195949e+00	1.093391e-01
  5 | L	3.890927e+00	-1.341657e-01	1477010445349642	4.291359e+00	2.153118e-01	2.284336e+00	2.263225e-01
  6 | R	4.200967e+00	5.202598e-02	2.418127e+00	1477010445349642	4.291359e+00	2.153118e-01	2.284336e+00	2.263225e-01
  7 | L	6.863517e+00	4.168175e-01	1477010446349642	6.569422e+00	4.960956e-01	2.363816e+00	3.488471e-01
  8 | R	6.456604e+00	7.330529e-02	2.438466e+00	1477010446349642	6.569422e+00	4.960956e-01	2.363816e+00	3.488471e-01
  9 | L	9.077331e+00	5.932112e-01	1477010447349642	8.924371e+00	8.992403e-01	2.433593e+00	4.745258e-01
 10 | R	8.941596e+00	9.936074e-02	2.529122e+00	1477010447349642	8.924371e+00	8.992403e-01	2.433593e+00	4.745258e-01
 11 | L	1.157555e+01	1.666900e+00	1477010448349642	1.134677e+01	1.426997e+00	2.493282e+00	6.007812e-01
 12 | R	1.153365e+01	1.242681e-01	2.500995e+00	1477010448349642	1.134677e+01	1.426997e+00	2.493282e+00	6.007812e-01
 13 | L	1.359209e+01	2.311915e+00	1477010449349642	1.382695e+01	2.079045e+00	2.542900e+00	7.249468e-01
 14 | R	1.380271e+01	1.453491e-01	2.539724e+00	1477010449349642	1.382695e+01	2.079045e+00	2.542900e+00	7.249468e-01
 15 | L	1.664559e+01	2.902999e+00	1477010450349642	1.635537e+01	2.852433e+00	2.582842e+00	8.443653e-01
 16 | R	1.652890e+01	1.730753e-01	2.728349e+00	1477010450349642	1.635537e+01	2.852433e+00	2.582842e+00	8.443653e-01
 17 | L	1.901058e+01	3.705553e+00	1477010451349642	1.892299e+01	3.741610e+00	2.613820e+00	9.564796e-01
 18 | R	1.974624e+01	1.973267e-01	2.502012e+00	1477010451349642	1.892299e+01	3.741610e+00	2.613820e+00	9.564796e-01
 19 | L	2.133124e+01	4.732053e+00	1477010452349642	2.152152e+01	4.738551e+00	2.636790e+00	1.058912e+00
 20 | R	2.213645e+01	2.161499e-01	2.798219e+00	1477010452349642	2.152152e+01	4.738551e+00	2.636790e+00	1.058912e+00
 21 | L	2.407871e+01	5.569675e+00	1477010453349642	2.414369e+01	5.832949e+00	2.652861e+00	1.149525e+00
 22 | R	2.526425e+01	2.366585e-01	3.081090e+00	1477010453349642	2.414369e+01	5.832949e+00	2.652861e+00	1.149525e+00
 23 | L	2.660525e+01	6.961020e+00	1477010454349642	2.678329e+01	7.012457e+00	2.663198e+00	1.226470e+00
 24 | R	2.742432e+01	2.543628e-01	2.977206e+00	1477010454349642	2.678329e+01	7.012457e+00	2.663198e+00	1.226470e+00
 25 | L	2.958639e+01	8.087890e+00	1477010455349642	2.943523e+01	8.262985e+00	2.668927e+00	1.288219e+00
 26 | R	3.060322e+01	2.727931e-01	3.000533e+00	1477010455349642	2.943523e+01	8.262985e+00	2.668927e+00	1.288219e+00
 27 | L	3.194160e+01	9.880168e+00	1477010456349642	3.209547e+01	9.569013e+00	2.671045e+00	1.333578e+00
 28 | R	3.318149e+01	2.903330e-01	2.789422e+00	1477010456349642	3.209547e+01	9.569013e+00	2.671045e+00	1.333578e+00
 29 | L	3.461568e+01	1.069880e+01	1477010457349642	3.476081e+01	1.091392e+01	2.670346e+00	1.361701e+00
 30 | R	3.674365e+01	3.039004e-01	2.988207e+00	1477010457349642	3.476081e+01	1.091392e+01	2.670346e+00	1.361701e+00
 31 | L	3.714036e+01	1.208833e+01	1477010458349642	3.742874e+01	1.228034e+01	2.667361e+00	1.372088e+00
 32 | R	3.946136e+01	3.168263e-01	3.028242e+00	1477010458349642	3.742874e+01	1.228034e+01	2.667361e+00	1.372088e+00
 33 | L	4.032853e+01	1.346661e+01	1477010459349642	4.009711e+01	1.365048e+01	2.662324e+00	1.364585e+00
 34 | R	4.216287e+01	3.290412e-01	2.947967e+00	1477010459349642	4.009711e+01	1.365048e+01	2.662324e+00	1.364585e+00
 35 | L	4.276639e+01	1.482937e+01	1477010460349642	4.276381e+01	1.500648e+01	2.655152e+00	1.339380e+00
 36 | R	4.513067e+01	3.383838e-01	3.104881e+00	1477010460349642	4.276381e+01	1.500648e+01	2.655152e+00	1.339380e+00
 37 | L	4.535721e+01	1.655239e+01	1477010461349642	4.542651e+01	1.633075e+01	2.645461e+00	1.297005e+00
 38 | R	4.854672e+01	3.472554e-01	3.079899e+00	1477010461349642	4.542651e+01	1.633075e+01	2.645461e+00	1.297005e+00
 39 | L	4.815409e+01	1.728347e+01	1477010462349642	4.808234e+01	1.760632e+01	2.632596e+00	1.238325e+00
 40 | R	5.103968e+01	3.506980e-01	2.752674e+00	1477010462349642	4.808234e+01	1.760632e+01	2.632596e+00	1.238325e+00
 41 | L	5.062278e+01	1.907211e+01	1477010463349642	5.072763e+01	1.881717e+01	2.615687e+00	1.164538e+00
 42 | R	5.394915e+01	3.556170e-01	2.885539e+00	1477010463349642	5.072763e+01	1.881717e+01	2.615687e+00	1.164538e+00
 43 | L	5.372678e+01	1.989821e+01	1477010464349642	5.335773e+01	1.994857e+01	2.593717e+00	1.077164e+00
 44 | R	5.709391e+01	3.553123e-01	2.718978e+00	1477010464349642	5.335773e+01	1.994857e+01	2.593717e+00	1.077164e+00
 45 | L	5.603223e+01	2.109230e+01	1477010465349642	5.596692e+01	2.098742e+01	2.565613e+00	9.780221e-01
 46 | R	5.967820e+01	3.593175e-01	2.678392e+00	1477010465349642	5.596692e+01	2.098742e+01	2.565613e+00	9.780221e-01
 47 | L	5.866320e+01	2.178207e+01	1477010466349642	5.854835e+01	2.192253e+01	2.530333e+00	8.692047e-01
 48 | R	6.256831e+01	3.589798e-01	2.817748e+00	1477010466349642	5.854835e+01	2.192253e+01	2.530333e+00	8.692047e-01
 49 | L	6.087224e+01	2.278765e+01	1477010467349642	6.109413e+01	2.274490e+01	2.486966e+00	7.530307e-01
 50 | R	6.484539e+01	3.566487e-01	2.671849e+00	1477010467349642	6.109413e+01	2.274490e+01	2.486966e+00	7.530307e-01
 51 | L	6.338208e+01	2.340690e+01	1477010468349642	6.359551e+01	2.344793e+01	2.434818e+00	6.319871e-01
 52 | R	6.802621e+01	3.565512e-01	2.503319e+00	1477010468349642	6.359551e+01	2.344793e+01	2.434818e+00	6.319871e-01
 53 | L	6.634029e+01	2.399586e+01	1477010469349642	6.604312e+01	2.402757e+01	2.373488e+00	5.086549e-01
 54 | R	7.010336e+01	3.487223e-01	2.230104e+00	1477010469349642	6.604312e+01	2.402757e+01	2.373488e+00	5.086549e-01
 55 | L	6.826205e+01	2.435817e+01	1477010470349642	6.842730e+01	2.448242e+01	2.302925e+00	3.856231e-01
 56 | R	7.295088e+01	3.440404e-01	2.502639e+00	1477010470349642	6.842730e+01	2.448242e+01	2.302925e+00	3.856231e-01
 57 | L	7.082748e+01	2.488439e+01	1477010471349642	7.073845e+01	2.481366e+01	2.223466e+00	2.653951e-01
 58 | R	7.491955e+01	3.355868e-01	2.202921e+00	1477010471349642	7.073845e+01	2.481366e+01	2.223466e+00	2.653951e-01
 59 | L	7.289226e+01	2.471354e+01	1477010472349642	7.296749e+01	2.502499e+01	2.135839e+00	1.502921e-01
 60 | R	7.724250e+01	3.300601e-01	2.095280e+00	1477010472349642	7.296749e+01	2.502499e+01	2.135839e+00	1.502921e-01
 61 | L	7.500209e+01	2.525945e+01	1477010473349642	7.510617e+01	2.512237e+01	2.041141e+00	4.236258e-02
 62 | R	7.907670e+01	3.235040e-01	2.015980e+00	1477010473349642	7.510617e+01	2.512237e+01	2.041141e+00	4.236258e-02
 63 | L	7.712245e+01	2.492967e+01	1477010474349642	7.714752e+01	2.511374e+01	1.940798e+00	-5.669785e-02
 64 | R	8.117997e+01	3.168645e-01	1.610124e+00	1477010474349642	7.714752e+01	2.511374e+01	1.940798e+00	-5.669785e-02
 65 | L	7.910890e+01	2.503687e+01	1477010475349642	7.908610e+01	2.500864e+01	1.836491e+00	-1.456073e-01
 66 | R	8.301602e+01	3.055133e-01	1.740946e+00	1477010475349642	7.908610e+01	2.500864e+01	1.836491e+00	-1.456073e-01
 67 | L	8.080743e+01	2.486882e+01	1477010476349642	8.091825e+01	2.481776e+01	1.730078e+00	-2.235364e-01
 68 | R	8.486818e+01	2.974122e-01	1.480564e+00	1477010476349642	8.091825e+01	2.481776e+01	1.730078e+00	-2.235364e-01
 69 | L	8.262158e+01	2.463932e+01	1477010477349642	8.264225e+01	2.455250e+01	1.623498e+00	-2.901201e-01
 70 | R	8.615829e+01	2.921110e-01	1.429528e+00	1477010477349642	8.264225e+01	2.455250e+01	1.623498e+00	-2.901201e-01
 71 | L	8.417335e+01	2.455042e+01	1477010478349642	8.425837e+01	2.422449e+01	1.518687e+00	-3.454441e-01
 72 | R	8.773126e+01	2.810227e-01	1.497477e+00	1477010478349642	8.425837e+01	2.422449e+01	1.518687e+00	-3.454441e-01
 73 | L	8.563776e+01	2.380798e+01	1477010479349642	8.576880e+01	2.384512e+01	1.417490e+00	-3.900057e-01
 74 | R	8.919568e+01	2.695749e-01	1.285113e+00	1477010479349642	8.576880e+01	2.384512e+01	1.417490e+00	-3.900057e-01
 75 | L	8.694492e+01	2.352704e+01	1477010480349642	8.717760e+01	2.342518e+01	1.321593e+00	-4.246540e-01
 76 | R	9.071713e+01	2.621807e-01	1.125894e+00	1477010480349642	8.717760e+01	2.342518e+01	1.321593e+00	-4.246540e-01
 77 | L	8.882267e+01	2.278691e+01	1477010481349642	8.849045e+01	2.297451e+01	1.232475e+00	-4.505145e-01
 78 | R	9.134890e+01	2.539530e-01	1.140412e+00	1477010481349642	8.849045e+01	2.297451e+01	1.232475e+00	-4.505145e-01
 79 | L	8.949667e+01	2.230817e+01	1477010482349642	8.971448e+01	2.250173e+01	1.151377e+00	-4.689038e-01
 80 | R	9.255422e+01	2.453672e-01	8.351101e-01	1477010482349642	8.971448e+01	2.250173e+01	1.151377e+00	-4.689038e-01
 81 | L	9.051327e+01	2.166767e+01	1477010483349642	9.085799e+01	2.201409e+01	1.079294e+00	-4.812415e-01
 82 | R	9.349083e+01	2.386165e-01	1.002329e+00	1477010483349642	9.085799e+01	2.201409e+01	1.079294e+00	-4.812415e-01
 83 | L	9.171782e+01	2.111524e+01	1477010484349642	9.193022e+01	2.151741e+01	1.016984e+00	-4.889635e-01
 84 | R	9.453330e+01	2.297548e-01	8.770099e-01	1477010484349642	9.193022e+01	2.151741e+01	1.016984e+00	-4.889635e-01
 85 | L	9.285989e+01	2.095372e+01	1477010485349642	9.294111e+01	2.101603e+01	9.649925e-01	-4.934427e-01
 86 | R	9.566793e+01	2.220744e-01	8.834777e-01	1477010485349642	9.294111e+01	2.101603e+01	9.649925e-01	-4.934427e-01
 87 | L	9.346683e+01	2.021548e+01	1477010486349642	9.390109e+01	2.051295e+01	9.236899e-01	-4.959185e-01
 88 | R	9.595920e+01	2.153676e-01	8.327357e-01	1477010486349642	9.390109e+01	2.051295e+01	9.236899e-01	-4.959185e-01
 89 | L	9.532289e+01	1.965849e+01	1477010487349642	9.482094e+01	2.000996e+01	8.933078e-01	-4.974393e-01
 90 | R	9.656365e+01	2.074150e-01	6.844031e-01	1477010487349642	9.482094e+01	2.000996e+01	8.933078e-01	-4.974393e-01
 91 | L	9.550389e+01	1.969840e+01	1477010488349642	9.571161e+01	1.950782e+01	8.739797e-01	-4.988160e-01
 92 | R	9.759407e+01	2.020017e-01	7.770975e-01	1477010488349642	9.571161e+01	1.950782e+01	8.739797e-01	-4.988160e-01
 93 | L	9.626405e+01	1.915973e+01	1477010489349642	9.658418e+01	1.900655e+01	8.657734e-01	-5.005895e-01
 94 | R	9.838528e+01	1.929804e-01	7.375068e-01	1477010489349642	9.658418e+01	1.900655e+01	8.657734e-01	-5.005895e-01
 95 | L	9.752863e+01	1.872696e+01	1477010490349642	9.744996e+01	1.850597e+01	8.687159e-01	-5.030091e-01
 96 | R	9.921197e+01	1.842182e-01	8.430504e-01	1477010490349642	9.744996e+01	1.850597e+01	8.687159e-01	-5.030091e-01
 97 | L	9.821047e+01	1.837390e+01	1477010491349642	9.831977e+01	1.800486e+01	8.828045e-01	-5.060226e-01
 98 | R	1.000768e+02	1.824270e-01	7.431117e-01	1477010491349642	9.831977e+01	1.800486e+01	8.828045e-01	-5.060226e-01
 99 | L	9.903627e+01	1.735698e+01	1477010492349642	9.920492e+01	1.750296e+01	9.080060e-01	-5.092777e-01
100 | R	1.004778e+02	1.729878e-01	9.239015e-01	1477010492349642	9.920492e+01	1.750296e+01	9.080060e-01	-5.092777e-01
101 | L	9.999686e+01	1.698822e+01	1477010493349642	1.001165e+02	1.700014e+01	9.442419e-01	-5.121341e-01
102 | R	1.018611e+02	1.672018e-01	8.250369e-01	1477010493349642	1.001165e+02	1.700014e+01	9.442419e-01	-5.121341e-01
103 | L	1.012038e+02	1.653684e+01	1477010494349642	1.010655e+02	1.649696e+01	9.913622e-01	-5.136871e-01
104 | R	1.024956e+02	1.610466e-01	8.779700e-01	1477010494349642	1.010655e+02	1.649696e+01	9.913622e-01	-5.136871e-01
105 | L	1.023868e+02	1.639923e+01	1477010495349642	1.020628e+02	1.599493e+01	1.049110e+00	-5.128034e-01
106 | R	1.033017e+02	1.539040e-01	9.479811e-01	1477010495349642	1.020628e+02	1.599493e+01	1.049110e+00	-5.128034e-01
107 | L	1.030034e+02	1.535302e+01	1477010496349642	1.031189e+02	1.549673e+01	1.117082e+00	-5.081689e-01
108 | R	1.038820e+02	1.460638e-01	1.026220e+00	1477010496349642	1.031189e+02	1.549673e+01	1.117082e+00	-5.081689e-01
109 | L	1.041144e+02	1.527083e+01	1477010497349642	1.042439e+02	1.500637e+01	1.194686e+00	-4.983491e-01
110 | R	1.049482e+02	1.411149e-01	1.161765e+00	1477010497349642	1.042439e+02	1.500637e+01	1.194686e+00	-4.983491e-01
111 | L	1.055409e+02	1.444125e+01	1477010498349642	1.054471e+02	1.452929e+01	1.281109e+00	-4.818613e-01
112 | R	1.062427e+02	1.321827e-01	1.155763e+00	1477010498349642	1.054471e+02	1.452929e+01	1.281109e+00	-4.818613e-01
113 | L	1.068312e+02	1.404819e+01	1477010499349642	1.067373e+02	1.407238e+01	1.375293e+00	-4.572554e-01
114 | R	1.073871e+02	1.329968e-01	1.377422e+00	1477010499349642	1.067373e+02	1.407238e+01	1.375293e+00	-4.572554e-01
115 | L	1.080836e+02	1.366040e+01	1477010500349642	1.081217e+02	1.364396e+01	1.475922e+00	-4.232023e-01
116 | R	1.089939e+02	1.258840e-01	1.488510e+00	1477010500349642	1.081217e+02	1.364396e+01	1.475922e+00	-4.232023e-01
117 | L	1.095344e+02	1.357136e+01	1477010501349642	1.096065e+02	1.325356e+01	1.581440e+00	-3.785832e-01
118 | R	1.101759e+02	1.196914e-01	1.444469e+00	1477010501349642	1.096065e+02	1.325356e+01	1.581440e+00	-3.785832e-01
119 | L	1.110630e+02	1.258139e+01	1477010502349642	1.111961e+02	1.291178e+01	1.690076e+00	-3.225744e-01
120 | R	1.121764e+02	1.157520e-01	1.628898e+00	1477010502349642	1.111961e+02	1.291178e+01	1.690076e+00	-3.225744e-01
121 | L	1.129126e+02	1.248773e+01	1477010503349642	1.128933e+02	1.262989e+01	1.799903e+00	-2.547234e-01
122 | R	1.135236e+02	1.132060e-01	1.753766e+00	1477010503349642	1.128933e+02	1.262989e+01	1.799903e+00	-2.547234e-01
123 | L	1.148010e+02	1.255057e+01	1477010504349642	1.146988e+02	1.241950e+01	1.908905e+00	-1.750069e-01
124 | R	1.155748e+02	1.067891e-01	1.804447e+00	1477010504349642	1.146988e+02	1.241950e+01	1.908905e+00	-1.750069e-01
125 | L	1.166871e+02	1.210355e+01	1477010505349642	1.166112e+02	1.229211e+01	2.015069e+00	-8.386825e-02
126 | R	1.174924e+02	1.028766e-01	2.104336e+00	1477010505349642	1.166112e+02	1.229211e+01	2.015069e+00	-8.386825e-02
127 | L	1.189528e+02	1.200094e+01	1477010506349642	1.186276e+02	1.225864e+01	2.116475e+00	1.777120e-02
128 | R	1.194957e+02	1.040097e-01	2.118656e+00	1477010506349642	1.186276e+02	1.225864e+01	2.116475e+00	1.777120e-02
129 | L	1.206951e+02	1.229180e+01	1477010507349642	1.207427e+02	1.232894e+01	2.211388e+00	1.285280e-01
130 | R	1.215471e+02	1.004631e-01	2.152026e+00	1477010507349642	1.207427e+02	1.232894e+01	2.211388e+00	1.285280e-01
131 | L	1.235682e+02	1.282970e+01	1477010508349642	1.229501e+02	1.251140e+01	2.298350e+00	2.465990e-01
132 | R	1.238025e+02	1.033269e-01	2.308969e+00	1477010508349642	1.229501e+02	1.251140e+01	2.298350e+00	2.465990e-01
133 | L	1.248938e+02	1.277429e+01	1477010509349642	1.252417e+02	1.281250e+01	2.376243e+00	3.698274e-01
134 | R	1.260477e+02	1.032906e-01	2.561986e+00	1477010509349642	1.252417e+02	1.281250e+01	2.376243e+00	3.698274e-01
135 | L	1.278531e+02	1.332727e+01	1477010510349642	1.276087e+02	1.323651e+01	2.444340e+00	4.957869e-01
136 | R	1.281830e+02	1.027923e-01	2.533509e+00	1477010510349642	1.276087e+02	1.323651e+01	2.444340e+00	4.957869e-01
137 | L	1.299473e+02	1.369630e+01	1477010511349642	1.300415e+02	1.378526e+01	2.502323e+00	6.218785e-01
138 | R	1.310610e+02	1.053446e-01	2.395812e+00	1477010511349642	1.300415e+02	1.378526e+01	2.502323e+00	6.218785e-01
139 | L	1.320919e+02	1.450702e+01	1477010512349642	1.325304e+02	1.445798e+01	2.550277e+00	7.454300e-01
140 | R	1.332209e+02	1.078673e-01	2.602114e+00	1477010512349642	1.325304e+02	1.445798e+01	2.550277e+00	7.454300e-01
141 | L	1.350735e+02	1.504748e+01	1477010513349642	1.350660e+02	1.525130e+01	2.588654e+00	8.637952e-01
142 | R	1.359193e+02	1.080497e-01	2.575841e+00	1477010513349642	1.350660e+02	1.525130e+01	2.588654e+00	8.637952e-01
143 | L	1.375606e+02	1.621470e+01	1477010514349642	1.376394e+02	1.615922e+01	2.618214e+00	9.744419e-01
144 | R	1.386403e+02	1.194011e-01	2.882208e+00	1477010514349642	1.376394e+02	1.615922e+01	2.618214e+00	9.744419e-01
145 | L	1.403314e+02	1.700802e+01	1477010515349642	1.402424e+02	1.717333e+01	2.639944e+00	1.075029e+00
146 | R	1.413966e+02	1.243534e-01	2.863862e+00	1477010515349642	1.402424e+02	1.717333e+01	2.639944e+00	1.075029e+00
147 | L	1.428884e+02	1.799183e+01	1477010516349642	1.428679e+02	1.828295e+01	2.654969e+00	1.163464e+00
148 | R	1.437753e+02	1.265470e-01	2.833240e+00	1477010516349642	1.428679e+02	1.828295e+01	2.654969e+00	1.163464e+00
149 | L	1.453920e+02	1.916167e+01	1477010517349642	1.455099e+02	1.947543e+01	2.664454e+00	1.237952e+00
150 | R	1.470716e+02	1.321468e-01	2.905169e+00	1477010517349642	1.455099e+02	1.947543e+01	2.664454e+00	1.237952e+00
151 | L	1.482509e+02	2.047536e+01	1477010518349642	1.481635e+02	2.073641e+01	2.669511e+00	1.297019e+00
152 | R	1.496186e+02	1.404008e-01	2.754945e+00	1477010518349642	1.481635e+02	2.073641e+01	2.669511e+00	1.297019e+00
153 | L	1.506859e+02	2.211423e+01	1477010519349642	1.508248e+02	2.205016e+01	2.671106e+00	1.339530e+00
154 | R	1.522205e+02	1.438054e-01	2.969367e+00	1477010519349642	1.508248e+02	2.205016e+01	2.671106e+00	1.339530e+00
155 | L	1.534220e+02	2.349086e+01	1477010520349642	1.534907e+02	2.339992e+01	2.669994e+00	1.364696e+00
156 | R	1.551664e+02	1.481995e-01	2.797327e+00	1477010520349642	1.534907e+02	2.339992e+01	2.669994e+00	1.364696e+00
157 | L	1.560466e+02	2.480844e+01	1477010521349642	1.561589e+02	2.476822e+01	2.666656e+00	1.372076e+00
158 | R	1.580103e+02	1.593196e-01	2.643090e+00	1477010521349642	1.561589e+02	2.476822e+01	2.666656e+00	1.372076e+00
159 | L	1.589049e+02	2.603078e+01	1477010522349642	1.588271e+02	2.613723e+01	2.661274e+00	1.361574e+00
160 | R	1.611563e+02	1.632386e-01	3.010232e+00	1477010522349642	1.588271e+02	2.613723e+01	2.661274e+00	1.361574e+00
161 | L	1.614116e+02	2.742561e+01	1477010523349642	1.614933e+02	2.748911e+01	2.653713e+00	1.333437e+00
162 | R	1.639112e+02	1.707092e-01	2.719047e+00	1477010523349642	1.614933e+02	2.748911e+01	2.653713e+00	1.333437e+00
163 | L	1.636251e+02	2.876827e+01	1477010524349642	1.641551e+02	2.880636e+01	2.643541e+00	1.288250e+00
164 | R	1.667132e+02	1.767602e-01	2.800348e+00	1477010524349642	1.641551e+02	2.880636e+01	2.643541e+00	1.288250e+00
165 | L	1.668278e+02	3.020623e+01	1477010525349642	1.668094e+02	3.007213e+01	2.630062e+00	1.226937e+00
166 | R	1.693747e+02	1.763346e-01	2.783027e+00	1477010525349642	1.668094e+02	3.007213e+01	2.630062e+00	1.226937e+00
167 | L	1.690972e+02	3.137172e+01	1477010526349642	1.694525e+02	3.127061e+01	2.612376e+00	1.150751e+00
168 | R	1.721064e+02	1.793748e-01	2.783685e+00	1477010526349642	1.694525e+02	3.127061e+01	2.612376e+00	1.150751e+00
169 | L	1.721752e+02	3.234041e+01	1477010527349642	1.720795e+02	3.238732e+01	2.589450e+00	1.061263e+00
170 | R	1.752070e+02	1.875434e-01	2.809252e+00	1477010527349642	1.720795e+02	3.238732e+01	2.589450e+00	1.061263e+00
171 | L	1.746430e+02	3.346252e+01	1477010528349642	1.746846e+02	3.340945e+01	2.560210e+00	9.603409e-01
172 | R	1.777403e+02	1.919932e-01	2.623035e+00	1477010528349642	1.746846e+02	3.340945e+01	2.560210e+00	9.603409e-01
173 | L	1.770301e+02	3.420094e+01	1477010529349642	1.772606e+02	3.432617e+01	2.523629e+00	8.501189e-01
174 | R	1.804147e+02	1.887235e-01	2.511529e+00	1477010529349642	1.772606e+02	3.432617e+01	2.523629e+00	8.501189e-01
175 | L	1.801454e+02	3.514229e+01	1477010530349642	1.797995e+02	3.512887e+01	2.478827e+00	7.329486e-01
176 | R	1.830143e+02	1.950816e-01	2.477528e+00	1477010530349642	1.797995e+02	3.512887e+01	2.478827e+00	7.329486e-01
177 | L	1.825994e+02	3.575303e+01	1477010531349642	1.822925e+02	3.581136e+01	2.425154e+00	6.113383e-01
178 | R	1.856231e+02	1.966203e-01	2.441744e+00	1477010531349642	1.822925e+02	3.581136e+01	2.425154e+00	6.113383e-01
179 | L	1.847319e+02	3.651547e+01	1477010532349642	1.847302e+02	3.637004e+01	2.362264e+00	4.878767e-01
180 | R	1.883345e+02	1.931176e-01	2.201386e+00	1477010532349642	1.847302e+02	3.637004e+01	2.362264e+00	4.878767e-01
181 | L	1.869463e+02	3.694045e+01	1477010533349642	1.871027e+02	3.680395e+01	2.290172e+00	3.651451e-01
182 | R	1.909034e+02	1.947161e-01	2.410297e+00	1477010533349642	1.871027e+02	3.680395e+01	2.290172e+00	3.651451e-01
183 | L	1.894652e+02	3.740516e+01	1477010534349642	1.894007e+02	3.711471e+01	2.209278e+00	2.456228e-01
184 | R	1.933027e+02	1.923400e-01	2.217294e+00	1477010534349642	1.894007e+02	3.711471e+01	2.209278e+00	2.456228e-01
185 | L	1.917353e+02	3.756307e+01	1477010535349642	1.916151e+02	3.730640e+01	2.120375e+00	1.315915e-01
186 | R	1.952425e+02	1.912593e-01	2.103453e+00	1477010535349642	1.916151e+02	3.730640e+01	2.120375e+00	1.315915e-01
187 | L	1.934712e+02	3.727891e+01	1477010536349642	1.937378e+02	3.738535e+01	2.024621e+00	2.504525e-02
188 | R	1.971401e+02	1.909903e-01	1.968279e+00	1477010536349642	1.937378e+02	3.738535e+01	2.024621e+00	2.504525e-02
189 | L	1.959914e+02	3.739908e+01	1477010537349642	1.957622e+02	3.735980e+01	1.923487e+00	-7.238600e-02
190 | R	1.990686e+02	1.894537e-01	1.776298e+00	1477010537349642	1.957622e+02	3.735980e+01	1.923487e+00	-7.238600e-02
191 | L	1.977246e+02	3.772855e+01	1477010538349642	1.976831e+02	3.723950e+01	1.818693e+00	-1.594945e-01
192 | R	2.010726e+02	1.866010e-01	1.872312e+00	1477010538349642	1.976831e+02	3.723950e+01	1.818693e+00	-1.594945e-01
193 | L	1.992941e+02	3.728617e+01	1477010539349642	1.994972e+02	3.703531e+01	1.712115e+00	-2.355288e-01
194 | R	2.028256e+02	1.852191e-01	1.586984e+00	1477010539349642	1.994972e+02	3.703531e+01	1.712115e+00	-2.355288e-01
195 | L	2.015491e+02	3.699733e+01	1477010540349642	2.012029e+02	3.675869e+01	1.605699e+00	-3.002014e-01
196 | R	2.043960e+02	1.843601e-01	1.497659e+00	1477010540349642	2.012029e+02	3.675869e+01	1.605699e+00	-3.002014e-01
197 | L	2.029624e+02	3.645683e+01	1477010541349642	2.028011e+02	3.642125e+01	1.501370e+00	-3.536706e-01
198 | R	2.060446e+02	1.817587e-01	1.261351e+00	1477010541349642	2.028011e+02	3.642125e+01	1.501370e+00	-3.536706e-01
199 | L	2.039906e+02	3.619670e+01	1477010542349642	2.042941e+02	3.603433e+01	1.400951e+00	-3.964974e-01
200 | R	2.070150e+02	1.762321e-01	1.441483e+00	1477010542349642	2.042941e+02	3.603433e+01	1.400951e+00	-3.964974e-01
201 | 


--------------------------------------------------------------------------------
/results/input2.log:
--------------------------------------------------------------------------------
   1 | EKF init: 0.0001
   2 | 0.0001
   3 |      0
   4 |      0
   5 | x_ =  0.000338488
   6 | -0.000446911
   7 |            0
   8 |            0
   9 | P_ = 0.155172 0.155172        0        0
  10 | 0.155172 0.155172        0        0
  11 |        0        0  500.225 -499.775
  12 |        0        0 -499.775  500.225
  13 | x_ =   1.55415
  14 | -0.143754
  15 |   2.01017
  16 |  0.309235
  17 | P_ =    0.0224163 -8.32143e-05     0.029637   0.00708695
  18 | -8.32143e-05    0.0224163   0.00708695     0.029637
  19 |     0.029637   0.00708695      1.81921    -0.448339
  20 |   0.00708695     0.029637    -0.448339      1.81921
  21 | x_ =   1.62109
  22 | 0.0192285
  23 |   2.30043
  24 |  0.494589
  25 | P_ =   0.0175592 -0.00145972  0.00199299   0.0109125
  26 | -0.00145972   0.0021331 -0.00047657   0.0020869
  27 |  0.00199299 -0.00047657   0.0949551    0.117373
  28 |   0.0109125   0.0020869    0.117373     1.58643
  29 | x_ =   3.89102
  30 | -0.130397
  31 |   2.26271
  32 | -0.525834
  33 | P_ =    0.0222877    6.939e-06    0.0433337 -0.000667391
  34 |    6.939e-06    0.0223688 -0.000736052    0.0354638
  35 |    0.0433337 -0.000736052     0.245313    0.0694785
  36 | -0.000667391    0.0354638    0.0694785     0.993706
  37 | x_ =   3.96437
  38 | 0.0739224
  39 |   2.42288
  40 | -0.197898
  41 | P_ =    0.0140284 -5.71777e-05   0.00980516  -0.00498394
  42 | -5.71777e-05   0.00846711  0.000773942    0.0137245
  43 |   0.00980516  0.000773942    0.0662493    0.0437968
  44 |  -0.00498394    0.0137245    0.0437968     0.954766
  45 | x_ =  6.85906
  46 | 0.413143
  47 |  3.33637
  48 | 0.702939
  49 | P_ =    0.0222866  2.58552e-06    0.0434035 -0.000260216
  50 |  2.58552e-06    0.0223448 -0.000218462     0.037709
  51 |    0.0434035 -0.000218462     0.239277     0.022021
  52 | -0.000260216     0.037709     0.022021     0.790269
  53 | x_ =   6.6903
  54 | 0.432431
  55 |  2.62654
  56 | 0.574797
  57 | P_ =    0.0140675 -0.000108183    0.0103764  -0.00761846
  58 | -0.000108183    0.0146318  -0.00172699    0.0241088
  59 |    0.0103764  -0.00172699    0.0652664   -0.0269811
  60 |  -0.00761846    0.0241088   -0.0269811      0.75254
  61 | x_ =   9.07965
  62 |  0.596255
  63 |   2.15901
  64 | -0.135533
  65 | P_ =    0.0222866 -2.51812e-06    0.0433969  0.000262365
  66 | -2.51812e-06     0.022336  0.000302869    0.0384516
  67 |    0.0433969  0.000302869     0.240369   -0.0312751
  68 |  0.000262365    0.0384516   -0.0312751     0.735372
  69 | x_ =    9.09362
  70 |   0.667446
  71 |    2.40714
  72 | 0.00382532
  73 | P_ =    0.0139837 -0.000149363    0.0101177  -0.00138392
  74 | -0.000149363    0.0171782  -0.00091109    0.0295058
  75 |    0.0101177  -0.00091109     0.066704    -0.041996
  76 |  -0.00138392    0.0295058    -0.041996     0.719393
  77 | x_ = 11.5747
  78 | 1.65959
  79 | 2.56527
  80 | 1.70838
  81 | P_ =    0.0222866 -3.09043e-06     0.043394  0.000309596
  82 | -3.09043e-06    0.0223349   0.00031389    0.0384977
  83 |     0.043394   0.00031389     0.240333   -0.0315579
  84 |  0.000309596    0.0384977   -0.0315579     0.739089
  85 | x_ = 11.5246
  86 | 1.61821
  87 | 2.36441
  88 | 1.59307
  89 | P_ =    0.0141775 -0.000874178    0.0113618  -0.00815145
  90 | -0.000874178    0.0187395  -0.00463284    0.0309389
  91 |    0.0113618  -0.00463284    0.0751627   -0.0838619
  92 |  -0.00815145    0.0309389   -0.0838619     0.710875
  93 | x_ =   13.5952
  94 |   2.31862
  95 |   1.76386
  96 | 0.0442223
  97 | P_ =    0.0222874 -6.76596e-06    0.0433068  0.000706921
  98 | -6.76596e-06    0.0223346  0.000728514     0.038515
  99 |    0.0433068  0.000728514     0.250129   -0.0764812
 100 |  0.000706921     0.038515   -0.0764812     0.740939
 101 | x_ =   13.6906
 102 |   2.29565
 103 |   2.32833
 104 | 0.0971446
 105 | P_ =    0.0141275 -0.000908099    0.0108567  -0.00501102
 106 | -0.000908099    0.0196109  -0.00371508    0.0331764
 107 |    0.0108567  -0.00371508    0.0822326    -0.109843
 108 |  -0.00501102    0.0331764    -0.109843     0.726351
 109 | x_ =  16.6395
 110 |  2.89904
 111 |  3.55283
 112 | 0.991102
 113 | P_ =   0.0222878 -8.2179e-06   0.0432724 0.000837889
 114 | -8.2179e-06   0.0223356 0.000847703   0.0383944
 115 |   0.0432724 0.000847703    0.253005  -0.0871101
 116 | 0.000837889   0.0383944  -0.0871101    0.755687
 117 | x_ =  16.4806
 118 |  2.87106
 119 |  2.85466
 120 | 0.861002
 121 | P_ =   0.0141864 -0.00115526   0.0109243 -0.00468135
 122 | -0.00115526   0.0203392 -0.00455139   0.0343142
 123 |   0.0109243 -0.00455139   0.0845474   -0.118226
 124 | -0.00468135   0.0343142   -0.118226    0.744508
 125 | x_ =  19.0137
 126 |  3.70587
 127 |  2.22932
 128 | 0.802891
 129 | P_ =    0.022288 -8.7794e-06   0.0432555  0.00090161
 130 | -8.7794e-06   0.0223366 0.000901738   0.0382777
 131 |   0.0432555 0.000901738    0.254817  -0.0935012
 132 |  0.00090161   0.0382777  -0.0935012    0.768947
 133 | x_ =  19.0872
 134 |  3.72615
 135 |  2.38026
 136 | 0.842603
 137 | P_ =   0.0142467 -0.00133941   0.0112811 -0.00603087
 138 | -0.00133941   0.0206846 -0.00507344   0.0345112
 139 |   0.0112811 -0.00507344   0.0893568   -0.132383
 140 | -0.00603087   0.0345112   -0.132383    0.755513
 141 | x_ = 21.3324
 142 | 4.73093
 143 | 2.12594
 144 | 1.11391
 145 | P_ =    0.0222884 -9.83167e-06    0.0432163   0.00100954
 146 | -9.83167e-06    0.0223372   0.00101552     0.038224
 147 |    0.0432163   0.00101552     0.259005    -0.105289
 148 |   0.00100954     0.038224    -0.105289     0.774793
 149 | x_ = 21.4286
 150 | 4.75053
 151 | 2.48866
 152 | 1.20798
 153 | P_ =   0.0143438 -0.00158072   0.0117346  -0.0071636
 154 | -0.00158072   0.0208721 -0.00588046   0.0344349
 155 |   0.0117346 -0.00588046   0.0967723   -0.150804
 156 |  -0.0071636   0.0344349   -0.150804      0.7585
 157 | x_ =  24.0774
 158 |  5.57241
 159 |  2.77833
 160 | 0.555692
 161 | P_ =    0.0222889 -1.11895e-05    0.0431572   0.00115025
 162 | -1.11895e-05    0.0223372   0.00115807    0.0382221
 163 |    0.0431572   0.00115807     0.265277    -0.120233
 164 |   0.00115025    0.0382221    -0.120233     0.775149
 165 | x_ =  24.1858
 166 |  5.60662
 167 |   3.0073
 168 | 0.624424
 169 | P_ =   0.0143521 -0.00163334   0.0114903 -0.00626052
 170 | -0.00163334   0.0210923 -0.00574419   0.0350451
 171 |   0.0114903 -0.00574419    0.100847    -0.16044
 172 | -0.00626052   0.0350451    -0.16044    0.762639
 173 | x_ = 26.6104
 174 | 6.95605
 175 | 1.91948
 176 | 1.83217
 177 | P_ =    0.0222891 -1.17405e-05    0.0431362   0.00120215
 178 | -1.17405e-05    0.0223374   0.00120586    0.0381941
 179 |    0.0431362   0.00120586     0.267149    -0.124772
 180 |   0.00120215    0.0381941    -0.124772     0.778635
 181 | x_ =  26.669
 182 | 6.97214
 183 | 2.35329
 184 | 1.98497
 185 | P_ =   0.0145189 -0.00195292   0.0124556 -0.00847955
 186 | -0.00195292   0.0210927 -0.00709311     0.03425
 187 |   0.0124556 -0.00709311    0.109667   -0.177957
 188 | -0.00847955     0.03425   -0.177957     0.75828
 189 | x_ =  29.5816
 190 |  8.09386
 191 |  3.38012
 192 | 0.541373
 193 | P_ =    0.0222899 -1.31633e-05     0.043056   0.00135527
 194 | -1.31633e-05    0.0223369     0.001363    0.0382476
 195 |     0.043056     0.001363     0.275991    -0.141801
 196 |   0.00135527    0.0382476    -0.141801     0.772767
 197 | x_ =  29.5271
 198 |  8.08402
 199 |  3.09614
 200 | 0.466634
 201 | P_ =    0.014502 -0.00195209   0.0120029 -0.00730943
 202 | -0.00195209   0.0212412 -0.00664522   0.0349676
 203 |   0.0120029 -0.00664522    0.115059   -0.188179
 204 | -0.00730943   0.0349676   -0.188179    0.757605
 205 | x_ = 31.9471
 206 | 9.87095
 207 |  1.8757
 208 |  2.6839
 209 | P_ =    0.0222902 -1.37195e-05    0.0430312   0.00140413
 210 | -1.37195e-05    0.0223369    0.0014092    0.0382437
 211 |    0.0430312    0.0014092     0.278022    -0.145787
 212 |   0.00140413    0.0382437    -0.145787     0.773906
 213 | x_ = 31.9327
 214 | 9.86053
 215 | 1.98511
 216 | 2.72836
 217 | P_ =   0.0147424 -0.00232106   0.0133887 -0.00986665
 218 | -0.00232106   0.0210956 -0.00827275   0.0336122
 219 |   0.0133887 -0.00827275    0.126816   -0.207034
 220 | -0.00986665   0.0336122   -0.207034    0.747299
 221 | x_ =   34.6105
 222 |   10.7121
 223 |   3.18342
 224 | -0.443884
 225 | P_ =    0.0222912 -1.52852e-05    0.0429217    0.0015744
 226 | -1.52852e-05     0.022336   0.00158296    0.0383443
 227 |    0.0429217   0.00158296     0.290173    -0.164801
 228 |    0.0015744    0.0383443    -0.164801     0.762793
 229 | x_ = 34.6942
 230 |  10.741
 231 | 3.28977
 232 | -0.4049
 233 | P_ =   0.0146212  -0.0021695   0.0121292 -0.00730113
 234 |  -0.0021695   0.0212806  -0.0071494   0.0350633
 235 |   0.0121292  -0.0071494    0.129371   -0.211416
 236 | -0.00730113   0.0350633   -0.211416    0.747984
 237 | x_ =  37.147
 238 | 12.0761
 239 |   1.802
 240 | 2.52021
 241 | P_ =   0.0222912 -1.5304e-05   0.0429338  0.00156327
 242 | -1.5304e-05   0.0223362  0.00156585   0.0383127
 243 |   0.0429338  0.00156585    0.287805    -0.16173
 244 |  0.00156327   0.0383127    -0.16173     0.76727
 245 | x_ = 37.2622
 246 | 12.1165
 247 | 2.20137
 248 | 2.66782
 249 | P_ =   0.0148061 -0.00243702   0.0132821 -0.00918254
 250 | -0.00243702   0.0211541  -0.0085356   0.0339595
 251 |   0.0132821  -0.0085356    0.134829   -0.219439
 252 | -0.00918254   0.0339595   -0.219439    0.744276
 253 | x_ =  40.3215
 254 |  13.4756
 255 |  3.75256
 256 | 0.484602
 257 | P_ =  0.0222917 -1.607e-05  0.0428714 0.00165316
 258 | -1.607e-05  0.0223358 0.00165569  0.0383652
 259 |  0.0428714 0.00165569   0.295113  -0.172229
 260 | 0.00165316  0.0383652  -0.172229   0.761059
 261 | x_ =  40.1874
 262 |  13.4356
 263 |   3.1988
 264 | 0.302444
 265 | P_ =   0.0147594 -0.00238642   0.0127817  -0.0084299
 266 | -0.00238642   0.0212556 -0.00793847   0.0345827
 267 |   0.0127817 -0.00793847     0.13895   -0.225412
 268 |  -0.0084299   0.0345827   -0.225412    0.741977
 269 | x_ = 42.7713
 270 | 14.8218
 271 | 2.09976
 272 |  2.1173
 273 | P_ =    0.0222919 -1.63658e-05    0.0428563    0.0016751
 274 | -1.63658e-05    0.0223357   0.00167919    0.0383689
 275 |    0.0428563   0.00167919     0.296094      -0.1738
 276 |    0.0016751    0.0383689      -0.1738     0.761345
 277 | x_ = 42.7953
 278 | 14.8338
 279 | 2.36692
 280 | 2.22852
 281 | P_ =   0.0148852 -0.00255406   0.0134358 -0.00929495
 282 | -0.00255406   0.0211656 -0.00876015   0.0339584
 283 |   0.0134358 -0.00876015    0.144004   -0.232189
 284 | -0.00929495   0.0339584   -0.232189    0.738022
 285 | x_ = 45.3558
 286 |  16.556
 287 | 2.69835
 288 | 1.37297
 289 | P_ =   0.0222923 -1.6938e-05   0.0428078   0.0017398
 290 | -1.6938e-05   0.0223353  0.00174228   0.0384112
 291 |   0.0428078  0.00174228    0.301553   -0.180984
 292 |   0.0017398   0.0384112   -0.180984    0.756568
 293 | x_ = 45.4025
 294 | 16.5716
 295 | 2.77683
 296 |  1.3981
 297 | P_ =   0.0149396 -0.00262451   0.0136367 -0.00968915
 298 | -0.00262451   0.0211446 -0.00881895   0.0338171
 299 |   0.0136367 -0.00881895    0.151362   -0.241456
 300 | -0.00968915   0.0338171   -0.241456    0.731408
 301 | x_ =  48.1549
 302 |  17.2885
 303 |   2.6736
 304 | 0.223198
 305 | P_ =    0.0222928 -1.75891e-05    0.0427552   0.00180372
 306 | -1.75891e-05    0.0223348   0.00180938    0.0384589
 307 |    0.0427552   0.00180938     0.306907    -0.187636
 308 |   0.00180372    0.0384589    -0.187636     0.751784
 309 | x_ =  48.1527
 310 |  17.2906
 311 |  2.76745
 312 | 0.259331
 313 | P_ =   0.0148889 -0.00257017   0.0129664 -0.00834041
 314 | -0.00257017   0.0212191 -0.00856517   0.0345273
 315 |   0.0129664 -0.00856517    0.150667   -0.240788
 316 | -0.00834041   0.0345273   -0.240788    0.733025
 317 | x_ = 50.6243
 318 | 19.0612
 319 | 2.32289
 320 | 2.83589
 321 | P_ =    0.0222927 -1.74277e-05    0.0427746   0.00178476
 322 | -1.74277e-05     0.022335   0.00178378    0.0384342
 323 |    0.0427746   0.00178378     0.304443    -0.184779
 324 |   0.00178476    0.0384342    -0.184779     0.754833
 325 | x_ = 50.5741
 326 | 19.0396
 327 | 2.12039
 328 | 2.74739
 329 | P_ =   0.0150421 -0.00275015   0.0140627  -0.0101008
 330 | -0.00275015   0.0210889 -0.00936843   0.0334408
 331 |   0.0140627 -0.00936843    0.156632   -0.247831
 332 |  -0.0101008   0.0334408   -0.247831    0.727254
 333 | x_ =   53.7188
 334 |   19.9113
 335 |    3.9236
 336 | -0.398974
 337 | P_ =    0.0222933 -1.80817e-05    0.0427105   0.00185762
 338 | -1.80817e-05    0.0223345   0.00186125    0.0384967
 339 |    0.0427105   0.00186125     0.311778    -0.193388
 340 |   0.00185762    0.0384967    -0.193388     0.747681
 341 | x_ =   53.6036
 342 |   19.8704
 343 |    3.3505
 344 | -0.596477
 345 | P_ =   0.0149214 -0.00261704   0.0129887 -0.00841258
 346 | -0.00261704   0.0212284 -0.00859525   0.0345616
 347 |   0.0129887 -0.00859525    0.155736   -0.246923
 348 | -0.00841258   0.0345616   -0.246923    0.728766
 349 | x_ = 56.0393
 350 | 21.0797
 351 | 1.74676
 352 |  2.4372
 353 | P_ =     0.022293 -1.78431e-05    0.0427408   0.00182535
 354 | -1.78431e-05    0.0223347   0.00182536    0.0384625
 355 |    0.0427408   0.00182536     0.307792    -0.188888
 356 |   0.00182535    0.0384625    -0.188888     0.752109
 357 | x_ = 56.0303
 358 | 21.0766
 359 | 1.84832
 360 | 2.47996
 361 | P_ =   0.0150375 -0.00275377    0.013814 -0.00955155
 362 | -0.00275377   0.0211264 -0.00936689   0.0337578
 363 |    0.013814 -0.00936689    0.157503   -0.249056
 364 | -0.00955155   0.0337578   -0.249056    0.727499
 365 | x_ =   58.6574
 366 |   21.7945
 367 |   3.19094
 368 | -0.490703
 369 | P_ =    0.0222933 -1.81198e-05    0.0427103   0.00186102
 370 | -1.81198e-05    0.0223345   0.00186114    0.0384901
 371 |    0.0427103   0.00186114     0.311612    -0.193354
 372 |   0.00186102    0.0384901    -0.193354     0.748609
 373 | x_ =   58.6557
 374 |   21.7952
 375 |    3.1878
 376 | -0.489578
 377 | P_ =   0.0149364  -0.0026402   0.0130736  -0.0085377
 378 |  -0.0026402   0.0212405 -0.00868534   0.0345193
 379 |   0.0130736 -0.00868534    0.156161   -0.247587
 380 |  -0.0085377   0.0345193   -0.247587    0.729228
 381 | x_ =   60.88
 382 | 22.7775
 383 | 1.46369
 384 | 1.96475
 385 | P_ =   0.0222931 -1.7898e-05   0.0427363   0.0018317
 386 | -1.7898e-05   0.0223347  0.00183173   0.0384618
 387 |   0.0427363  0.00183173    0.308327   -0.189628
 388 |   0.0018317   0.0384618   -0.189628    0.752155
 389 | x_ = 60.9212
 390 | 22.7934
 391 | 1.85305
 392 | 2.11731
 393 | P_ =   0.0150195 -0.00273851    0.013667 -0.00933322
 394 | -0.00273851   0.0211677 -0.00926745   0.0339441
 395 |    0.013667 -0.00926745     0.15691   -0.248521
 396 | -0.00933322   0.0339441   -0.248521    0.728815
 397 | x_ =   63.3777
 398 |   23.4175
 399 |   2.88317
 400 | -0.404352
 401 | P_ =   0.0222932 -1.8057e-05   0.0427177  0.00185375
 402 | -1.8057e-05   0.0223346  0.00185336   0.0384783
 403 |   0.0427177  0.00185336    0.310747   -0.192469
 404 |  0.00185375   0.0384783   -0.192469    0.749961
 405 | x_ =    63.438
 406 |   23.4411
 407 |   2.88902
 408 | -0.396882
 409 | P_ =   0.0149366 -0.00264405   0.0130892 -0.00855115
 410 | -0.00264405   0.0212604 -0.00871527    0.034535
 411 |   0.0130892 -0.00871527    0.155348   -0.246753
 412 | -0.00855115    0.034535   -0.246753    0.730605
 413 | x_ = 66.3394
 414 | 23.9889
 415 | 2.99152
 416 | 1.23058
 417 | P_ =     0.022293 -1.78416e-05    0.0427414   0.00182657
 418 | -1.78416e-05    0.0223348   0.00182629    0.0384533
 419 |    0.0427414   0.00182629     0.307845    -0.189137
 420 |   0.00182657    0.0384533    -0.189137     0.753025
 421 | x_ =  66.1698
 422 |   23.928
 423 |  2.27246
 424 | 0.979948
 425 | P_ =    0.014937 -0.00264624   0.0131291 -0.00848904
 426 | -0.00264624   0.0212704 -0.00886875   0.0345165
 427 |   0.0131291 -0.00886875    0.152272   -0.243171
 428 | -0.00848904   0.0345165   -0.243171    0.733898
 429 | x_ =   68.2641
 430 |   24.3623
 431 |   1.88592
 432 | 0.0264649
 433 | P_ =    0.0222928 -1.76068e-05    0.0427605   0.00180545
 434 | -1.76068e-05     0.022335   0.00180276    0.0384327
 435 |    0.0427605   0.00180276     0.306029    -0.187018
 436 |   0.00180545    0.0384327    -0.187018     0.755017
 437 | x_ =  68.4083
 438 |  24.4139
 439 |  2.41734
 440 | 0.211611
 441 | P_ =   0.0148986 -0.00260085   0.0130083  -0.0084242
 442 | -0.00260085   0.0213144 -0.00863633   0.0346437
 443 |   0.0130083 -0.00863633    0.150021   -0.240488
 444 |  -0.0084242   0.0346437   -0.240488    0.736352
 445 | x_ =  70.8273
 446 |  24.8825
 447 |  2.44139
 448 | 0.653677
 449 | P_ =    0.0222927 -1.74031e-05    0.0427784   0.00178318
 450 | -1.74031e-05    0.0223352   0.00178208    0.0384136
 451 |    0.0427784   0.00178208     0.304119    -0.184722
 452 |   0.00178318    0.0384136    -0.184722     0.757051
 453 | x_ =  70.7719
 454 |  24.8623
 455 |  2.21238
 456 | 0.574144
 457 | P_ =   0.0148828 -0.00258256   0.0129671  -0.0083463
 458 | -0.00258256   0.0213373 -0.00861026    0.034697
 459 |   0.0129671 -0.00861026    0.147744   -0.237713
 460 |  -0.0083463    0.034697   -0.237713    0.738779
 461 | x_ =   72.8937
 462 |   24.7189
 463 |   1.98072
 464 | -0.666701
 465 | P_ =    0.0222925 -1.72083e-05    0.0427943   0.00176399
 466 | -1.72083e-05    0.0223354   0.00176229    0.0383967
 467 |    0.0427943   0.00176229     0.302512    -0.182756
 468 |   0.00176399    0.0383967    -0.182756     0.758775
 469 | x_ =   72.9792
 470 |   24.7486
 471 |   2.30324
 472 | -0.563106
 473 | P_ =   0.0148101 -0.00248863   0.0125574 -0.00767165
 474 | -0.00248863   0.0214154 -0.00821851   0.0351062
 475 |   0.0125574 -0.00821851    0.143075   -0.231771
 476 | -0.00767165   0.0351062   -0.231771    0.743397
 477 | x_ = 75.0039
 478 | 25.2518
 479 | 1.85107
 480 |  1.2463
 481 | P_ =    0.0222921 -1.67335e-05    0.0428343    0.0017135
 482 | -1.67335e-05    0.0223357   0.00171043    0.0383569
 483 |    0.0428343   0.00171043     0.298176    -0.177201
 484 |    0.0017135    0.0383569    -0.177201     0.763103
 485 | x_ = 74.9802
 486 | 25.2434
 487 | 1.75283
 488 | 1.21262
 489 | P_ =   0.0148311 -0.00251835   0.0128835  -0.0082941
 490 | -0.00251835   0.0213987 -0.00844673   0.0347969
 491 |   0.0128835 -0.00844673    0.141457   -0.229705
 492 |  -0.0082941   0.0347969   -0.229705    0.745232
 493 | x_ =    77.12
 494 |  24.9405
 495 |   2.3784
 496 | -1.35949
 497 | P_ =    0.0222921 -1.66529e-05    0.0428378   0.00170838
 498 | -1.66529e-05    0.0223358   0.00170702    0.0383521
 499 |    0.0428378   0.00170702     0.298123    -0.177162
 500 |   0.00170838    0.0383521    -0.177162     0.763305
 501 | x_ =  77.1134
 502 |  24.9402
 503 |  2.23022
 504 | -1.39709
 505 | P_ =   0.0147304 -0.00238024   0.0122477 -0.00721206
 506 | -0.00238024   0.0215042 -0.00779001   0.0354237
 507 |   0.0122477 -0.00779001    0.136704   -0.223278
 508 | -0.00721206   0.0354237   -0.223278    0.749844
 509 | x_ =   79.11
 510 | 25.0262
 511 | 1.89208
 512 | 1.12899
 513 | P_ =    0.0222917 -1.60982e-05    0.0428835   0.00164716
 514 | -1.60982e-05    0.0223362   0.00164424    0.0383071
 515 |    0.0428835   0.00164424     0.293008    -0.170213
 516 |   0.00164716    0.0383071    -0.170213     0.768351
 517 | x_ = 79.0734
 518 | 25.0141
 519 | 1.62212
 520 | 1.04643
 521 | P_ =   0.0147412 -0.00239754   0.0125046 -0.00766618
 522 | -0.00239754   0.0214967 -0.00804042   0.0351894
 523 |   0.0125046 -0.00804042    0.133547   -0.218933
 524 | -0.00766618   0.0351894   -0.218933    0.753213
 525 | x_ = 80.8072
 526 | 24.8774
 527 | 1.74951
 528 | -0.9735
 529 | P_ =    0.0222915 -1.58545e-05    0.0428985   0.00162639
 530 | -1.58545e-05    0.0223364   0.00162357    0.0382907
 531 |    0.0428985   0.00162357     0.291766    -0.168504
 532 |   0.00162639    0.0382907    -0.168504     0.769748
 533 | x_ =   80.8647
 534 |   24.8947
 535 |   1.84799
 536 | -0.943153
 537 | P_ =   0.0146738 -0.00229972   0.0121734 -0.00717745
 538 | -0.00229972   0.0215671 -0.00755581   0.0355168
 539 |   0.0121734 -0.00755581    0.130245   -0.214184
 540 | -0.00717745   0.0355168   -0.214184     0.75658
 541 | x_ =   82.622
 542 |  24.6344
 543 |  1.72251
 544 | 0.219904
 545 | P_ =    0.0222912 -1.54649e-05     0.042928   0.00158325
 546 | -1.54649e-05    0.0223367   0.00158137    0.0382609
 547 |     0.042928   0.00158137     0.288513    -0.163791
 548 |   0.00158325    0.0382609    -0.163791     0.773033
 549 | x_ =  82.5827
 550 |  24.6234
 551 |  1.52136
 552 | 0.165049
 553 | P_ =   0.0146524 -0.00226768   0.0121378 -0.00706413
 554 | -0.00226768   0.0215917 -0.00754172   0.0355623
 555 |   0.0121378 -0.00754172    0.126608   -0.208786
 556 | -0.00706413   0.0355623   -0.208786    0.760292
 557 | x_ =   84.1729
 558 |   24.5521
 559 |   1.63723
 560 | -0.234727
 561 | P_ =     0.022291 -1.50928e-05    0.0429527   0.00154699
 562 | -1.50928e-05    0.0223369   0.00154385    0.0382356
 563 |    0.0429527   0.00154385      0.28605    -0.160104
 564 |   0.00154699    0.0382356    -0.160104      0.77556
 565 | x_ =  84.1797
 566 |  24.5533
 567 |  1.63712
 568 | -0.23578
 569 | P_ =   0.0146188 -0.00221623   0.0120604 -0.00702015
 570 | -0.00221623   0.0216275 -0.00733137   0.0356426
 571 |   0.0120604 -0.00733137    0.123936   -0.204689
 572 | -0.00702015   0.0356426   -0.204689    0.763072
 573 | x_ =  85.6398
 574 |  23.8118
 575 |  1.26079
 576 | -1.11328
 577 | P_ =    0.0222908 -1.47978e-05    0.0429722    0.0015161
 578 | -1.47978e-05    0.0223371   0.00151429    0.0382155
 579 |    0.0429722   0.00151429     0.284019    -0.156964
 580 |    0.0015161    0.0382155    -0.156964     0.777653
 581 | x_ =  85.7256
 582 |  23.8349
 583 |  1.54821
 584 | -1.04032
 585 | P_ =   0.0145569 -0.00211377   0.0117248 -0.00629633
 586 | -0.00211377   0.0216922 -0.00693695   0.0359742
 587 |   0.0117248 -0.00693695    0.119301   -0.197237
 588 | -0.00629633   0.0359742   -0.197237    0.767574
 589 | x_ =  86.9475
 590 |   23.522
 591 | 0.966863
 592 | 0.181192
 593 | P_ =    0.0222904 -1.42184e-05    0.0430103   0.00145549
 594 | -1.42184e-05    0.0223375   0.00145175    0.0381781
 595 |    0.0430103   0.00145175     0.279954    -0.150367
 596 |   0.00145549    0.0381781    -0.150367     0.781657
 597 | x_ =  87.0599
 598 |  23.5518
 599 |  1.13591
 600 | 0.227366
 601 | P_ =   0.0145438 -0.00209322   0.0118234 -0.00657443
 602 | -0.00209322   0.0217064 -0.00696562   0.0358876
 603 |   0.0118234 -0.00696562    0.116249   -0.192163
 604 | -0.00657443   0.0358876   -0.192163    0.770774
 605 | x_ =  88.8174
 606 |  22.7937
 607 |  2.27202
 608 | -1.41572
 609 | P_ =    0.0222902 -1.38974e-05    0.0430283   0.00142473
 610 | -1.38974e-05    0.0223377   0.00142209    0.0381592
 611 |    0.0430283   0.00142209     0.278249    -0.147505
 612 |   0.00142473    0.0381592    -0.147505      0.78346
 613 | x_ = 88.6883
 614 | 22.7621
 615 | 1.74243
 616 | -1.5345
 617 | P_ =   0.0144697 -0.00195948   0.0114273 -0.00572485
 618 | -0.00195948   0.0217835 -0.00637453   0.0362747
 619 |   0.0114273 -0.00637453    0.111698   -0.184155
 620 | -0.00572485   0.0362747   -0.184155    0.775173
 621 | x_ =   89.5048
 622 |   22.3009
 623 | 0.0195024
 624 |  0.240042
 625 | P_ =    0.0222899 -1.32572e-05    0.0430673    0.0013561
 626 | -1.32572e-05     0.022338   0.00135252     0.038121
 627 |    0.0430673   0.00135252     0.274019    -0.139987
 628 |    0.0013561     0.038121    -0.139987     0.787611
 629 | x_ =   89.634
 630 |  22.3336
 631 | 0.577943
 632 |   0.3734
 633 | P_ =   0.0144668 -0.00195609   0.0115852 -0.00612679
 634 | -0.00195609   0.0217865 -0.00652759   0.0361318
 635 |   0.0115852 -0.00652759    0.108829   -0.178932
 636 | -0.00612679   0.0361318   -0.178932     0.77823
 637 | x_ =   90.511
 638 |   21.675
 639 |  1.09378
 640 | -1.36904
 641 | P_ =    0.0222897 -1.29439e-05    0.0430827   0.00132743
 642 | -1.29439e-05    0.0223382   0.00132458    0.0381044
 643 |    0.0430827   0.00132458     0.272632    -0.137441
 644 |   0.00132743    0.0381044    -0.137441     0.789115
 645 | x_ =  90.6023
 646 |  21.6977
 647 |  1.31756
 648 | -1.31553
 649 | P_ =   0.0144112  -0.0018477    0.011316 -0.00556898
 650 |  -0.0018477   0.0218434 -0.00602312   0.0363941
 651 |    0.011316 -0.00602312      0.1057   -0.172941
 652 | -0.00556898   0.0363941   -0.172941    0.781361
 653 | x_ =   91.7193
 654 |   21.1101
 655 |  0.971842
 656 | -0.086387
 657 | P_ =   0.0222894 -1.2465e-05   0.0431096  0.00127543
 658 | -1.2465e-05   0.0223385  0.00127303   0.0380777
 659 |   0.0431096  0.00127303    0.269709   -0.131792
 660 |  0.00127543   0.0380777   -0.131792     0.79203
 661 | x_ =    91.7765
 662 |    21.1242
 663 |   0.980908
 664 | -0.0793717
 665 | P_ =   0.0143936 -0.00181233   0.0113081  -0.0055263
 666 | -0.00181233   0.0218625 -0.00600308   0.0364106
 667 |   0.0113081 -0.00600308     0.10271   -0.166989
 668 |  -0.0055263   0.0364106   -0.166989    0.784418
 669 | x_ =    92.859
 670 |   20.9543
 671 |    1.1723
 672 | -0.227902
 673 | P_ =    0.0222892 -1.20601e-05    0.0431295   0.00123592
 674 | -1.20601e-05    0.0223387   0.00123279    0.0380573
 675 |    0.0431295   0.00123279     0.267741    -0.127831
 676 |   0.00123592    0.0380573    -0.127831      0.79405
 677 | x_ =   92.9079
 678 |   20.9655
 679 |   1.07757
 680 | -0.245694
 681 | P_ =   0.0143749 -0.00177429   0.0113035 -0.00563372
 682 | -0.00177429   0.0218822 -0.00586267   0.0364174
 683 |   0.0113035 -0.00586267    0.101036   -0.163543
 684 | -0.00563372   0.0364174   -0.163543    0.786215
 685 | x_ =    93.472
 686 |   20.2194
 687 | 0.0560218
 688 |   -1.1264
 689 | P_ =    0.0222891 -1.18244e-05    0.0431408   0.00121155
 690 | -1.18244e-05    0.0223388   0.00121019    0.0380454
 691 |    0.0431408   0.00121019     0.266596    -0.125453
 692 |   0.00121155    0.0380454    -0.125453      0.79526
 693 | x_ =  93.6323
 694 |   20.254
 695 | 0.800877
 696 | -0.98001
 697 | P_ =   0.0143388  -0.0016949   0.0111102 -0.00509564
 698 |  -0.0016949   0.0219192 -0.00554604   0.0366074
 699 |   0.0111102 -0.00554604   0.0984125    -0.15792
 700 | -0.00509564   0.0366074    -0.15792    0.788803
 701 | x_ =   95.3143
 702 |   19.6553
 703 |   2.52761
 704 | -0.284208
 705 | P_ =   0.0222889 -1.1386e-05   0.0431624  0.00116553
 706 | -1.1386e-05    0.022339  0.00116292   0.0380239
 707 |   0.0431624  0.00116292    0.264281   -0.120449
 708 |  0.00116553   0.0380239   -0.120449    0.797555
 709 | x_ =   95.0094
 710 |   19.5941
 711 |   1.23197
 712 | -0.524167
 713 | P_ =   0.0143105 -0.00163008   0.0110196 -0.00484238
 714 | -0.00163008   0.0219497 -0.00535277   0.0367029
 715 |   0.0110196 -0.00535277   0.0954769   -0.151339
 716 | -0.00484238   0.0367029   -0.151339    0.791719
 717 | x_ =   95.5105
 718 |   19.6943
 719 | -0.152303
 720 |  0.500752
 721 | P_ =    0.0222887 -1.09117e-05    0.0431838   0.00111739
 722 | -1.09117e-05    0.0223392   0.00111423    0.0380025
 723 |    0.0431838   0.00111423     0.262079    -0.115453
 724 |   0.00111739    0.0380025    -0.115453     0.799765
 725 | x_ =  95.6121
 726 |  19.7152
 727 | 0.430349
 728 | 0.621647
 729 | P_ =   0.0143206 -0.00165506   0.0112066 -0.00551369
 730 | -0.00165506   0.0219393  -0.0055427   0.0365194
 731 |   0.0112066  -0.0055427   0.0949914   -0.150238
 732 | -0.00551369   0.0365194   -0.150238     0.79235
 733 | x_ =  96.2625
 734 |   19.168
 735 | 0.797252
 736 | -1.35544
 737 | P_ =    0.0222887 -1.08968e-05     0.043183   0.00111807
 738 | -1.08968e-05    0.0223392   0.00111765    0.0380026
 739 |     0.043183   0.00111765     0.262317    -0.116015
 740 |   0.00111807    0.0380026    -0.116015       0.7996
 741 | x_ =  96.3223
 742 |  19.1789
 743 |  0.97708
 744 | -1.32316
 745 | P_ =   0.0142813 -0.00156063   0.0109467 -0.00474743
 746 | -0.00156063   0.0219796 -0.00506988    0.036775
 747 |   0.0109467 -0.00506988   0.0932824   -0.146195
 748 | -0.00474743    0.036775   -0.146195    0.794034
 749 | x_ =  97.5261
 750 |  18.7206
 751 |  1.45885
 752 | 0.158666
 753 | P_ =    0.0222885 -1.05374e-05    0.0432001     0.001078
 754 | -1.05374e-05    0.0223394   0.00107642    0.0379857
 755 |    0.0432001   0.00107642      0.26038    -0.111415
 756 |     0.001078    0.0379857    -0.111415      0.80152
 757 | x_ =  97.4439
 758 |  18.7035
 759 |  1.01503
 760 | 0.077536
 761 | P_ =   0.0142726 -0.00153927   0.0109527 -0.00472436
 762 | -0.00153927   0.0219897 -0.00510094   0.0367761
 763 |   0.0109527 -0.00510094   0.0913319   -0.141443
 764 | -0.00472436   0.0367761   -0.141443    0.796017
 765 | x_ =    98.213
 766 |   18.3769
 767 |  0.518857
 768 | -0.621104
 769 | P_ =    0.0222884 -1.02186e-05    0.0432128   0.00104739
 770 | -1.02186e-05    0.0223395   0.00104486    0.0379727
 771 |    0.0432128   0.00104486     0.259139    -0.108364
 772 |   0.00104739    0.0379727    -0.108364     0.802794
 773 | x_ =  98.2756
 774 |  18.3879
 775 | 0.779833
 776 | -0.57614
 777 | P_ =   0.0142526 -0.00148879   0.0108992 -0.00464888
 778 | -0.00148879   0.0220101  -0.0048907   0.0368293
 779 |   0.0108992  -0.0048907   0.0899105   -0.137853
 780 | -0.00464888   0.0368293   -0.137853    0.797489
 781 | x_ =  99.0367
 782 |  17.3602
 783 | 0.722475
 784 | -1.34425
 785 | P_ =    0.0222883 -9.95584e-06    0.0432235   0.00101961
 786 | -9.95584e-06    0.0223396   0.00101825    0.0379618
 787 |    0.0432235   0.00101825     0.258018    -0.105509
 788 |   0.00101961    0.0379618    -0.105509      0.80394
 789 | x_ =  99.0751
 790 |  17.3664
 791 |  1.02804
 792 | -1.30161
 793 | P_ =   0.0142157 -0.00138641    0.010691 -0.00390319
 794 | -0.00138641   0.0220485 -0.00448333   0.0370328
 795 |    0.010691 -0.00448333   0.0871404   -0.130494
 796 | -0.00390319   0.0370328   -0.130494     0.80011
 797 | x_ =  99.9975
 798 |  16.9817
 799 | 0.863034
 800 | 0.250894
 801 | P_ =    0.0222881 -9.38027e-06    0.0432463  0.000959297
 802 | -9.38027e-06    0.0223398   0.00095592    0.0379396
 803 |    0.0432463   0.00095592     0.255568   -0.0989247
 804 |  0.000959297    0.0379396   -0.0989247     0.806315
 805 | x_ =  100.056
 806 |  16.9914
 807 | 0.862789
 808 | 0.252413
 809 | P_ =   0.0142116 -0.00137721   0.0107785 -0.00427183
 810 | -0.00137721   0.0220528 -0.00457719   0.0369548
 811 |   0.0107785 -0.00457719   0.0855334   -0.126043
 812 | -0.00427183   0.0369548   -0.126043     0.80183
 813 | x_ =   101.201
 814 |   16.5418
 815 |   1.38105
 816 | -0.927518
 817 | P_ =     0.022288 -9.11256e-06    0.0432551   0.00093435
 818 | -9.11256e-06    0.0223399  0.000932213    0.0379302
 819 |    0.0432551  0.000932213     0.254779   -0.0967141
 820 |   0.00093435    0.0379302   -0.0967141     0.807178
 821 | x_ =   101.157
 822 |   16.5345
 823 |   1.13968
 824 | -0.963097
 825 | P_ =   0.0141858 -0.00130114   0.0106618  -0.0039198
 826 | -0.00130114   0.0220798 -0.00422989   0.0370696
 827 |   0.0106618 -0.00422989   0.0840157   -0.121641
 828 |  -0.0039198   0.0370696   -0.121641     0.80338
 829 | x_ =  102.386
 830 |  16.3933
 831 |  1.34601
 832 | 0.435614
 833 | P_ =   0.0222879 -8.7662e-06   0.0432678 0.000896953
 834 | -8.7662e-06     0.02234  0.00089529   0.0379174
 835 |   0.0432678  0.00089529    0.253403  -0.0926864
 836 | 0.000896953   0.0379174  -0.0926864    0.808568
 837 | x_ =  102.277
 838 |  16.3746
 839 | 0.987376
 840 | 0.377426
 841 | P_ =   0.0141866 -0.00130453   0.0107222 -0.00415687
 842 | -0.00130453     0.02208 -0.00434586   0.0370144
 843 |   0.0107222 -0.00434586    0.083128   -0.118998
 844 | -0.00415687   0.0370144   -0.118998    0.804351
 845 | x_ =  103.006
 846 |  15.3631
 847 | 0.431207
 848 | -1.99018
 849 | P_ =    0.0222878 -8.61308e-06    0.0432723  0.000883356
 850 | -8.61308e-06    0.0223401  0.000881973    0.0379124
 851 |    0.0432723  0.000881973     0.253013   -0.0915199
 852 |  0.000883356    0.0379124   -0.0915199     0.809011
 853 | x_ =  103.065
 854 |  15.3701
 855 |  1.03393
 856 | -1.92442
 857 | P_ =   0.0141465 -0.00117339   0.0104795 -0.00320465
 858 | -0.00117339   0.0221211 -0.00372926   0.0372474
 859 |   0.0104795 -0.00372926   0.0809247   -0.112061
 860 | -0.00320465   0.0372474   -0.112061    0.806393
 861 | x_ = 104.114
 862 | 15.2579
 863 | 1.13046
 864 | 1.14999
 865 | P_ =    0.0222876 -8.03598e-06    0.0432923  0.000820629
 866 | -8.03598e-06    0.0223403  0.000817846    0.0378933
 867 |    0.0432923  0.000817846      0.25081   -0.0845127
 868 |  0.000820629    0.0378933   -0.0845127     0.811122
 869 | x_ = 104.057
 870 | 15.2486
 871 | 1.01316
 872 | 1.13102
 873 | P_ =   0.0141584 -0.00121603   0.0106488 -0.00392783
 874 | -0.00121603   0.0221096 -0.00410667   0.0370907
 875 |   0.0106488 -0.00410667    0.080089   -0.109354
 876 | -0.00392783   0.0370907   -0.109354    0.807362
 877 | x_ = 105.537
 878 | 14.4548
 879 | 1.84801
 880 | -2.1162
 881 | P_ =    0.0222876 -7.93101e-06     0.043294  0.000814546
 882 | -7.93101e-06    0.0223403  0.000812933    0.0378907
 883 |     0.043294  0.000812933     0.250793   -0.0844766
 884 |  0.000814546    0.0378907   -0.0844766     0.811228
 885 | x_ =  105.451
 886 |  14.4425
 887 |  1.54307
 888 | -2.15345
 889 | P_ =   0.0141177 -0.00107151   0.0104009 -0.00292218
 890 | -0.00107151   0.0221519 -0.00337047   0.0373293
 891 |   0.0104009 -0.00337047   0.0783047   -0.103197
 892 | -0.00292218   0.0373293   -0.103197     0.80904
 893 | x_ =  106.832
 894 |  14.0358
 895 |  1.28789
 896 | 0.802138
 897 | P_ =    0.0222875 -7.39342e-06    0.0433114  0.000754364
 898 | -7.39342e-06    0.0223405   0.00075221     0.037874
 899 |    0.0433114   0.00075221     0.248835   -0.0776603
 900 |  0.000754364     0.037874   -0.0776603     0.813124
 901 | x_ =  106.776
 902 |   14.029
 903 |  1.24487
 904 | 0.796471
 905 | P_ =   0.0141224 -0.00109103    0.010501 -0.00333484
 906 | -0.00109103   0.0221481 -0.00365766   0.0372427
 907 |    0.010501 -0.00365766   0.0770903  -0.0988381
 908 | -0.00333484   0.0372427  -0.0988381    0.810373
 909 | x_ =  108.083
 910 |  13.6686
 911 |   1.3283
 912 | -1.16224
 913 | P_ =    0.0222874 -7.14974e-06    0.0433171  0.000733823
 914 | -7.14974e-06    0.0223405  0.000731367    0.0378675
 915 |    0.0433171  0.000731367     0.248364   -0.0759593
 916 |  0.000733823    0.0378675   -0.0759593     0.813679
 917 | x_ =  108.126
 918 |  13.6738
 919 |  1.55755
 920 | -1.13673
 921 | P_ =   0.0141012 -0.00100962   0.0104076  -0.0030142
 922 | -0.00100962   0.0221703 -0.00324941   0.0373334
 923 |   0.0104076 -0.00324941   0.0761103  -0.0951232
 924 |  -0.0030142   0.0373334  -0.0951232    0.811407
 925 | x_ = 109.535
 926 | 13.5641
 927 | 1.30224
 928 | 0.59887
 929 | P_ =    0.0222873 -6.84703e-06    0.0433258  0.000700008
 930 | -6.84703e-06    0.0223406  0.000698901    0.0378585
 931 |    0.0433258  0.000698901     0.247406   -0.0722946
 932 |  0.000700008    0.0378585   -0.0722946      0.81466
 933 | x_ =  109.514
 934 |  13.5607
 935 |  1.33611
 936 | 0.601308
 937 | P_ =    0.014105 -0.00102546   0.0104649 -0.00327464
 938 | -0.00102546   0.0221677 -0.00343106   0.0372811
 939 |   0.0104649 -0.00343106   0.0755649  -0.0930053
 940 | -0.00327464   0.0372811  -0.0930053    0.812032
 941 | x_ =  111.061
 942 |  12.5925
 943 |  1.69752
 944 | -2.05147
 945 | P_ =    0.0222873 -6.73584e-06    0.0433281  0.000691184
 946 | -6.73584e-06    0.0223407  0.000689947    0.0378558
 947 |    0.0433281  0.000689947     0.247236   -0.0716326
 948 |  0.000691184    0.0378558   -0.0716326      0.81488
 949 | x_ =  111.143
 950 |  12.6024
 951 |  1.86465
 952 | -2.03162
 953 | P_ =    0.0140737 -0.000889853    0.0102703  -0.00227295
 954 | -0.000889853    0.0222006  -0.00277497    0.0374671
 955 |    0.0102703  -0.00277497    0.0739378   -0.0862626
 956 |  -0.00227295    0.0374671   -0.0862626     0.813501
 957 | x_ = 112.913
 958 | 12.4742
 959 | 1.73515
 960 | 1.18981
 961 | P_ =   0.0222871 -6.1654e-06   0.0433432 0.000628694
 962 | -6.1654e-06   0.0223408 0.000626085   0.0378415
 963 |   0.0433432 0.000626085    0.245561  -0.0646069
 964 | 0.000628694   0.0378415  -0.0646069     0.81647
 965 | x_ =  112.89
 966 | 12.4723
 967 | 1.65476
 968 | 1.18219
 969 | P_ =     0.014082 -0.000930772     0.010394  -0.00295986
 970 | -0.000930772    0.0221931  -0.00315196    0.0373567
 971 |     0.010394  -0.00315196    0.0732589   -0.0833397
 972 |  -0.00295986    0.0373567   -0.0833397      0.81431
 973 | x_ =  114.799
 974 |  12.5583
 975 |  2.11831
 976 | -0.66721
 977 | P_ =    0.0222871 -6.04353e-06    0.0433449  0.000620943
 978 | -6.04353e-06    0.0223408  0.000619261     0.037839
 979 |    0.0433449  0.000619261     0.245504    -0.064381
 980 |  0.000620943     0.037839    -0.064381     0.816617
 981 | x_ =   114.788
 982 |   12.5567
 983 |   1.96667
 984 | -0.683374
 985 | P_ =    0.0140711 -0.000882035    0.0103475  -0.00282498
 986 | -0.000882035    0.0222054  -0.00286668    0.0374038
 987 |    0.0103475  -0.00286668    0.0730107   -0.0822338
 988 |  -0.00282498    0.0374038   -0.0822338     0.814649
 989 | x_ =  116.688
 990 |  12.1019
 991 |  1.84354
 992 | -0.29812
 993 | P_ =    0.0222871 -5.93806e-06    0.0433477  0.000607608
 994 | -5.93806e-06    0.0223408  0.000607645    0.0378357
 995 |    0.0433477  0.000607645     0.245174   -0.0629025
 996 |  0.000607608    0.0378357   -0.0629025     0.816994
 997 | x_ =   116.749
 998 |   12.1081
 999 |   2.07684
1000 | -0.276336
1001 | P_ =     0.014064 -0.000847473    0.0103008   -0.0024769
1002 | -0.000847473    0.0222141  -0.00276751    0.0374519
1003 |    0.0103008  -0.00276751    0.0722359   -0.0786601
1004 |   -0.0024769    0.0374519   -0.0786601     0.815439
1005 | x_ =   118.952
1006 |   11.9997
1007 |    2.3262
1008 | 0.0113285
1009 | P_ =     0.022287 -5.66685e-06    0.0433538  0.000580207
1010 | -5.66685e-06    0.0223409  0.000578388    0.0378295
1011 |    0.0433538  0.000578388     0.244534   -0.0599246
1012 |  0.000580207    0.0378295   -0.0599246     0.817648
1013 | x_ =      118.92
1014 |     11.9973
1015 |     2.18003
1016 | -0.00132765
1017 | P_ =    0.0140599 -0.000828722    0.0103084  -0.00257574
1018 | -0.000828722    0.0222194  -0.00272979    0.0374474
1019 |    0.0103084  -0.00272979    0.0717309   -0.0762413
1020 |  -0.00257574    0.0374474   -0.0762413     0.815995
1021 | x_ =  120.699
1022 |  12.2898
1023 |  1.40639
1024 | 0.485827
1025 | P_ =     0.022287 -5.50785e-06     0.043357  0.000564358
1026 | -5.50785e-06    0.0223409  0.000563379     0.037826
1027 |     0.043357  0.000563379     0.244221   -0.0584079
1028 |  0.000564358     0.037826   -0.0584079     0.817996
1029 | x_ =  120.812
1030 |  12.3012
1031 |    1.929
1032 | 0.540118
1033 | P_ =    0.0140633 -0.000845414     0.010349  -0.00286981
1034 | -0.000845414    0.0222171  -0.00283166    0.0374085
1035 |     0.010349  -0.00283166    0.0717767    -0.076467
1036 |  -0.00286981    0.0374085    -0.076467     0.815996
1037 | x_ =   123.56
1038 |  12.8296
1039 |  3.52325
1040 | 0.541531
1041 | P_ =     0.022287 -5.55045e-06    0.0433558  0.000569593
1042 | -5.55045e-06    0.0223409  0.000569737    0.0378269
1043 |    0.0433558  0.000569737     0.244372    -0.059148
1044 |  0.000569593    0.0378269    -0.059148     0.817873
1045 | x_ =  123.356
1046 |  12.8083
1047 |  2.59162
1048 | 0.440987
1049 | P_ =   0.0140659 -0.00085781    0.010363 -0.00296879
1050 | -0.00085781   0.0222163 -0.00286464   0.0373973
1051 |    0.010363 -0.00286464   0.0720519   -0.077796
1052 | -0.00296879   0.0373973   -0.077796    0.815751
1053 | x_ =   124.904
1054 |   12.7779
1055 |  0.548536
1056 | -0.384718
1057 | P_ =     0.022287 -5.64889e-06    0.0433537  0.000579507
1058 | -5.64889e-06    0.0223409  0.000580163    0.0378289
1059 |    0.0433537  0.000580163     0.244592   -0.0602068
1060 |  0.000579507    0.0378289   -0.0602068     0.817678
1061 | x_ =   125.208
1062 |   12.8089
1063 |   2.05789
1064 | -0.236286
1065 | P_ =   0.0140613 -0.00083583   0.0103199 -0.00267547
1066 | -0.00083583   0.0222216 -0.00274401   0.0374413
1067 |   0.0103199 -0.00274401   0.0719072  -0.0771016
1068 | -0.00267547   0.0374413  -0.0771016    0.815923
1069 | x_ = 127.847
1070 | 13.3218
1071 | 3.20799
1072 | 1.04732
1073 | P_ =     0.022287 -5.57326e-06    0.0433556  0.000570861
1074 | -5.57326e-06    0.0223409  0.000570538    0.0378268
1075 |    0.0433556  0.000570538     0.244367   -0.0591244
1076 |  0.000570861    0.0378268   -0.0591244      0.81792
1077 | x_ =  127.707
1078 |  13.3068
1079 |  2.62711
1080 | 0.983489
1081 | P_ =    0.0140686 -0.000870903    0.0103765  -0.00302278
1082 | -0.000870903    0.0222159   -0.0029392    0.0373869
1083 |    0.0103765   -0.0029392    0.0721179   -0.0781109
1084 |  -0.00302278    0.0373869   -0.0781109      0.81573
1085 | x_ =    129.951
1086 |    13.7006
1087 |    1.86588
1088 | -0.0251618
1089 | P_ =     0.022287 -5.67932e-06    0.0433529  0.000583271
1090 | -5.67932e-06    0.0223409  0.000583613    0.0378293
1091 |    0.0433529  0.000583613     0.244682   -0.0606338
1092 |  0.000583271    0.0378293   -0.0606338     0.817622
1093 | x_ =   130.072
1094 |   13.7133
1095 |   2.29301
1096 | 0.0204279
1097 | P_ =    0.0140672 -0.000864098    0.0103584  -0.00293067
1098 | -0.000864098    0.0222185  -0.00285873    0.0374073
1099 |    0.0103584  -0.00285873    0.0723184   -0.0790629
1100 |  -0.00293067    0.0374073   -0.0790629     0.815558
1101 | x_ = 132.094
1102 | 14.5016
1103 |  1.7878
1104 | 1.31347
1105 | P_ =     0.022287 -5.73091e-06     0.043352  0.000587408
1106 | -5.73091e-06    0.0223409  0.000587972      0.03783
1107 |     0.043352  0.000587972     0.244752   -0.0609653
1108 |  0.000587408      0.03783   -0.0609653     0.817574
1109 | x_ = 132.221
1110 | 14.5156
1111 | 2.30611
1112 | 1.37509
1113 | P_ =    0.0140798 -0.000921356    0.0104344  -0.00334463
1114 | -0.000921356     0.022207  -0.00313462    0.0373312
1115 |    0.0104344  -0.00313462    0.0729353   -0.0819018
1116 |  -0.00334463    0.0373312   -0.0819018     0.814929
1117 | x_ =    135.069
1118 |    15.0533
1119 |    3.33499
1120 | -0.0279259
1121 | P_ =    0.0222871 -5.97069e-06    0.0433463  0.000613678
1122 | -5.97069e-06    0.0223408  0.000614768    0.0378359
1123 |    0.0433463  0.000614768     0.245397   -0.0639145
1124 |  0.000613678    0.0378359   -0.0639145     0.816927
1125 | x_ =    134.989
1126 |    15.0441
1127 |    2.81698
1128 | -0.0876768
1129 | P_ =     0.014077 -0.000909595     0.010393   -0.0031033
1130 | -0.000909595     0.022211  -0.00300095    0.0373767
1131 |     0.010393  -0.00300095    0.0732704   -0.0834136
1132 |   -0.0031033    0.0373767   -0.0834136     0.814632
1133 | x_ = 137.563
1134 | 16.2059
1135 | 2.37858
1136 | 2.02155
1137 | P_ =    0.0222871 -6.04582e-06     0.043345  0.000619524
1138 | -6.04582e-06    0.0223408  0.000620348    0.0378369
1139 |     0.043345  0.000620348     0.245483   -0.0643041
1140 |  0.000619524    0.0378369   -0.0643041     0.816871
1141 | x_ = 137.613
1142 | 16.2124
1143 | 2.59233
1144 | 2.05063
1145 | P_ =    0.0140973 -0.000994529    0.0105142  -0.00371863
1146 | -0.000994529    0.0221921  -0.00341329    0.0372534
1147 |    0.0105142  -0.00341329    0.0742248   -0.0875303
1148 |  -0.00371863    0.0372534   -0.0875303     0.813631
1149 | x_ =    140.331
1150 |    17.0169
1151 |     2.7975
1152 | -0.0567298
1153 | P_ =    0.0222872 -6.39554e-06    0.0433359   0.00065795
1154 | -6.39554e-06    0.0223408  0.000659521    0.0378461
1155 |    0.0433359  0.000659521     0.246493   -0.0686265
1156 |   0.00065795    0.0378461   -0.0686265      0.81584
1157 | x_ =    140.347
1158 |    17.0195
1159 |    2.86758
1160 | -0.0468062
1161 | P_ =    0.0140953 -0.000987754    0.0104673  -0.00346442
1162 | -0.000987754    0.0221951  -0.00326536    0.0373064
1163 |    0.0104673  -0.00326536    0.0748986   -0.0903451
1164 |  -0.00346442    0.0373064   -0.0903451      0.81301
1165 | x_ = 142.891
1166 | 17.9847
1167 |  2.2704
1168 | 1.65791
1169 | P_ =    0.0222872 -6.55626e-06    0.0433326   0.00067195
1170 | -6.55626e-06    0.0223407  0.000673431    0.0378491
1171 |    0.0433326  0.000673431     0.246779   -0.0698144
1172 |   0.00067195    0.0378491   -0.0698144     0.815593
1173 | x_ = 142.895
1174 | 17.9857
1175 | 2.50637
1176 | 1.69291
1177 | P_ =   0.0141118 -0.00105276     0.01055 -0.00380075
1178 | -0.00105276   0.0221796 -0.00357762   0.0372227
1179 |     0.01055 -0.00357762   0.0757124  -0.0936011
1180 | -0.00380075   0.0372227  -0.0936011    0.812205
1181 | x_ = 145.392
1182 | 19.1653
1183 | 2.47212
1184 | 0.82285
1185 | P_ =    0.0222873 -6.82164e-06    0.0433253  0.000701105
1186 | -6.82164e-06    0.0223406  0.000702239    0.0378564
1187 |    0.0433253  0.000702239     0.247583   -0.0730062
1188 |  0.000701105    0.0378564   -0.0730062     0.814795
1189 | x_ =  145.496
1190 |  19.1792
1191 |  2.76299
1192 | 0.863239
1193 | P_ =   0.0141204 -0.00108501   0.0105806 -0.00395148
1194 | -0.00108501   0.0221721 -0.00364009   0.0371949
1195 |   0.0105806 -0.00364009   0.0767808  -0.0977126
1196 | -0.00395148   0.0371949  -0.0977126    0.811143
1197 | x_ = 148.251
1198 | 20.4723
1199 | 2.76243
1200 |  1.5916
1201 | P_ =    0.0222874 -7.11291e-06    0.0433176  0.000730062
1202 | -7.11291e-06    0.0223406  0.000732056     0.037864
1203 |    0.0433176  0.000732056     0.248369   -0.0759988
1204 |  0.000730062     0.037864   -0.0759988     0.814021
1205 | x_ = 148.223
1206 | 20.4689
1207 | 2.61793
1208 | 1.57028
1209 | P_ =   0.0141372 -0.00114535   0.0106495 -0.00419369
1210 | -0.00114535   0.0221564 -0.00387588   0.0371263
1211 |   0.0106495 -0.00387588    0.078007   -0.102205
1212 | -0.00419369   0.0371263   -0.102205    0.809913
1213 | x_ = 150.687
1214 | 22.1137
1215 | 2.32262
1216 | 1.69142
1217 | P_ =    0.0222875 -7.44927e-06    0.0433079   0.00076528
1218 | -7.44927e-06    0.0223405  0.000767124    0.0378737
1219 |    0.0433079  0.000767124     0.249394    -0.079713
1220 |   0.00076528    0.0378737    -0.079713     0.812998
1221 | x_ = 150.722
1222 | 22.1187
1223 | 2.61229
1224 | 1.74079
1225 | P_ =   0.0141576 -0.00121383   0.0107414 -0.00455258
1226 | -0.00121383    0.022137 -0.00411249   0.0370348
1227 |   0.0107414 -0.00411249    0.079708   -0.108105
1228 | -0.00455258   0.0370348   -0.108105    0.808169
1229 | x_ = 153.421
1230 | 23.4934
1231 | 2.76778
1232 | 1.12316
1233 | P_ =    0.0222876 -7.88821e-06    0.0432946  0.000810469
1234 | -7.88821e-06    0.0223404  0.000813119    0.0378871
1235 |    0.0432946  0.000813119     0.250792   -0.0844969
1236 |  0.000810469    0.0378871   -0.0844969     0.811585
1237 | x_ = 153.403
1238 |   23.49
1239 | 2.68699
1240 | 1.10874
1241 | P_ =   0.0141701 -0.00125486   0.0107598 -0.00454529
1242 | -0.00125486   0.0221252 -0.00421002   0.0370202
1243 |   0.0107598 -0.00421002   0.0811647   -0.112905
1244 | -0.00454529   0.0370202   -0.112905    0.806756
1245 | x_ = 156.047
1246 |  24.807
1247 | 2.61185
1248 | 1.46035
1249 | P_ =   0.0222877 -8.2178e-06   0.0432846 0.000843524
1250 | -8.2178e-06   0.0223403 0.000845637   0.0378969
1251 |   0.0432846 0.000845637    0.251806  -0.0877982
1252 | 0.000843524   0.0378969  -0.0877982      0.8106
1253 | x_ = 156.029
1254 | 24.8044
1255 | 2.49616
1256 | 1.44054
1257 | P_ =   0.0141863  -0.0013054   0.0108149 -0.00468872
1258 |  -0.0013054   0.0221098 -0.00439135   0.0369657
1259 |   0.0108149 -0.00439135   0.0825039   -0.117124
1260 | -0.00468872   0.0369657   -0.117124    0.805451
1261 | x_ = 158.901
1262 | 26.0322
1263 | 3.21775
1264 | 1.09458
1265 | P_ =    0.0222878 -8.52484e-06    0.0432745  0.000875304
1266 | -8.52484e-06    0.0223402  0.000877069    0.0379068
1267 |    0.0432745  0.000877069     0.252862   -0.0910923
1268 |  0.000875304    0.0379068   -0.0910923     0.809564
1269 | x_ = 158.885
1270 | 26.0295
1271 | 2.99321
1272 |  1.0546
1273 | P_ =   0.0141966 -0.00133648   0.0108355 -0.00472139
1274 | -0.00133648   0.0221003 -0.00446865   0.0369473
1275 |   0.0108355 -0.00446865   0.0836659   -0.120657
1276 | -0.00472139   0.0369473   -0.120657    0.804332
1277 | x_ = 161.416
1278 | 27.4234
1279 | 2.11036
1280 | 1.61147
1281 | P_ =    0.0222879 -8.77034e-06    0.0432663  0.000899975
1282 | -8.77034e-06    0.0223401  0.000901563    0.0379147
1283 |    0.0432663  0.000901563     0.253689    -0.093583
1284 |  0.000899975    0.0379147    -0.093583     0.808764
1285 | x_ = 161.484
1286 | 27.4355
1287 | 2.38818
1288 | 1.66276
1289 | P_ =   0.0142158 -0.00139184   0.0109208 -0.00498696
1290 | -0.00139184   0.0220818 -0.00468406   0.0368617
1291 |   0.0109208 -0.00468406   0.0850724   -0.124776
1292 | -0.00498696   0.0368617   -0.124776    0.802928
1293 | x_ = 163.628
1294 | 28.7707
1295 | 1.89893
1296 | 1.09629
1297 | P_ =     0.022288 -9.08156e-06    0.0432551  0.000932536
1298 | -9.08156e-06      0.02234  0.000934297    0.0379259
1299 |    0.0432551  0.000934297     0.254881   -0.0970444
1300 |  0.000932536    0.0379259   -0.0970444     0.807575
1301 | x_ = 163.797
1302 |  28.801
1303 | 2.47969
1304 | 1.20438
1305 | P_ =   0.0142296 -0.00143036   0.0109613 -0.00509341
1306 | -0.00143036   0.0220686 -0.00478784   0.0368229
1307 |   0.0109613 -0.00478784   0.0865351    -0.12891
1308 | -0.00509341   0.0368229    -0.12891     0.80148
1309 | x_ = 166.823
1310 | 30.2046
1311 | 3.54767
1312 | 1.56655
1313 | P_ =    0.0222881 -9.37452e-06    0.0432446  0.000962105
1314 | -9.37452e-06    0.0223399   0.00096401    0.0379364
1315 |    0.0432446   0.00096401     0.255963    -0.100077
1316 |  0.000962105    0.0379364    -0.100077     0.806505
1317 | x_ =  166.69
1318 | 30.1798
1319 | 2.84112
1320 |  1.4289
1321 | P_ =   0.0142452 -0.00147244   0.0110097 -0.00519804
1322 | -0.00147244   0.0220538 -0.00493178   0.0367757
1323 |   0.0110097 -0.00493178   0.0878936   -0.132618
1324 | -0.00519804   0.0367757   -0.132618    0.800144
1325 | x_ = 169.101
1326 | 31.3736
1327 | 1.99744
1328 | 1.01011
1329 | P_ =    0.0222882 -9.64181e-06    0.0432345  0.000989618
1330 | -9.64181e-06    0.0223398  0.000991219    0.0379463
1331 |    0.0432345  0.000991219     0.257011    -0.102913
1332 |  0.000989618    0.0379463    -0.102913     0.805472
1333 | x_ = 169.189
1334 | 31.3895
1335 | 2.45422
1336 | 1.09854
1337 | P_ =   0.0142567 -0.00150283   0.0110374 -0.00524719
1338 | -0.00150283   0.0220428 -0.00501798   0.0367494
1339 |   0.0110374 -0.00501798   0.0890893    -0.13579
1340 | -0.00524719   0.0367494    -0.13579    0.798978
1341 | x_ =   172.17
1342 |  32.3412
1343 |   3.4687
1344 | 0.873502
1345 | P_ =    0.0222883 -9.86482e-06     0.043226    0.0010122
1346 | -9.86482e-06    0.0223397   0.00101362    0.0379547
1347 |     0.043226   0.00101362     0.257883    -0.105209
1348 |    0.0010122    0.0379547    -0.105209     0.804618
1349 | x_ =  172.091
1350 |  32.3265
1351 |  2.94005
1352 | 0.771587
1353 | P_ =   0.0142617 -0.00151603   0.0110264 -0.00516368
1354 | -0.00151603   0.0220384 -0.00504049   0.0367619
1355 |   0.0110264 -0.00504049   0.0897975   -0.137632
1356 | -0.00516368   0.0367619   -0.137632     0.79831
1357 | x_ = 174.646
1358 | 33.4601
1359 | 2.21206
1360 | 1.36879
1361 | P_ =    0.0222883 -9.98462e-06    0.0432215   0.00102407
1362 | -9.98462e-06    0.0223396   0.00102486    0.0379589
1363 |    0.0432215   0.00102486     0.258318    -0.106337
1364 |   0.00102407    0.0379589    -0.106337     0.804207
1365 | x_ = 174.655
1366 | 33.4622
1367 | 2.33731
1368 | 1.39513
1369 | P_ =   0.0142761 -0.00155245   0.0110987 -0.00537497
1370 | -0.00155245   0.0220245 -0.00519215   0.0366895
1371 |   0.0110987 -0.00519215    0.090744   -0.140043
1372 | -0.00537497   0.0366895   -0.140043    0.797373
1373 | x_ =   177.03
1374 |  34.2056
1375 |  2.37946
1376 | 0.289327
1377 | P_ =    0.0222884 -1.01725e-05    0.0432136   0.00104396
1378 | -1.01725e-05    0.0223396     0.001045    0.0379667
1379 |    0.0432136     0.001045     0.259167    -0.108489
1380 |   0.00104396    0.0379667    -0.108489     0.803364
1381 | x_ =   177.06
1382 |  34.2111
1383 |  2.47569
1384 | 0.307569
1385 | P_ =   0.0142758 -0.00155193    0.011061 -0.00523423
1386 | -0.00155193   0.0220252 -0.00514189   0.0367288
1387 |    0.011061 -0.00514189   0.0912753   -0.141381
1388 | -0.00523423   0.0367288   -0.141381     0.79687
1389 | x_ = 180.139
1390 | 35.1376
1391 | 3.67553
1392 |  1.3884
1393 | P_ =   0.0222884 -1.0249e-05    0.043211  0.00105083
1394 | -1.0249e-05   0.0223395   0.0010515   0.0379692
1395 |    0.043211   0.0010515    0.259396   -0.109067
1396 |  0.00105083   0.0379692   -0.109067    0.803152
1397 | x_ = 179.907
1398 | 35.0923
1399 | 2.64508
1400 | 1.18316
1401 | P_ =   0.0142855 -0.00157587    0.011106 -0.00534279
1402 | -0.00157587   0.0220162 -0.00525488   0.0366844
1403 |    0.011106 -0.00525488   0.0917791   -0.142632
1404 | -0.00534279   0.0366844   -0.142632    0.796381
1405 | x_ =  182.599
1406 |  35.7567
1407 |   2.7111
1408 | 0.303646
1409 | P_ =   0.0222885 -1.0349e-05   0.0432066  0.00106177
1410 | -1.0349e-05   0.0223395  0.00106223   0.0379735
1411 |   0.0432066  0.00106223    0.259875   -0.110251
1412 |  0.00106177   0.0379735   -0.110251    0.802684
1413 | x_ =  182.501
1414 |   35.738
1415 |  2.46886
1416 | 0.256896
1417 | P_ =   0.0142836  -0.0015714   0.0110763 -0.00524812
1418 |  -0.0015714   0.0220185 -0.00520303    0.036715
1419 |   0.0110763 -0.00520303   0.0920335   -0.143261
1420 | -0.00524812    0.036715   -0.143261    0.796144
1421 | x_ = 184.734
1422 | 36.5119
1423 | 2.03648
1424 | 1.12429
1425 | P_ =    0.0222885 -1.03809e-05    0.0432056    0.0010643
1426 | -1.03809e-05    0.0223395   0.00106466    0.0379744
1427 |    0.0432056   0.00106466     0.259946    -0.110428
1428 |    0.0010643    0.0379744    -0.110428     0.802624
1429 | x_ = 184.736
1430 | 36.5121
1431 | 2.02947
1432 | 1.12235
1433 | P_ =   0.0142936 -0.00159568   0.0111331 -0.00540657
1434 | -0.00159568   0.0220089 -0.00532135   0.0366584
1435 |   0.0111331 -0.00532135   0.0924974   -0.144396
1436 | -0.00540657   0.0366584   -0.144396    0.795686
1437 | x_ =    186.945
1438 |    36.9453
1439 |     2.3426
1440 | -0.0404633
1441 | P_ =    0.0222885 -1.04769e-05    0.0432012   0.00107491
1442 | -1.04769e-05    0.0223395   0.00107534    0.0379787
1443 |    0.0432012   0.00107534     0.260431    -0.111614
1444 |   0.00107491    0.0379787    -0.111614     0.802143
1445 | x_ =   187.01
1446 |  36.9582
1447 |  2.46388
1448 | -0.01657
1449 | P_ =    0.014288 -0.00158225   0.0110787 -0.00523613
1450 | -0.00158225   0.0220148 -0.00522753   0.0367136
1451 |   0.0110787 -0.00522753     0.09257   -0.144574
1452 | -0.00523613   0.0367136   -0.144574    0.795625
1453 | x_ =  189.465
1454 |  37.4019
1455 |  2.46873
1456 | 0.765419
1457 | P_ =    0.0222885 -1.04702e-05     0.043202   0.00107323
1458 | -1.04702e-05    0.0223395    0.0010734    0.0379779
1459 |     0.043202    0.0010734     0.260307    -0.111312
1460 |   0.00107323    0.0379779    -0.111312     0.802275
1461 | x_ =  189.454
1462 |  37.3993
1463 |  2.24249
1464 | 0.720097
1465 | P_ =   0.0142911 -0.00158984   0.0110963 -0.00526723
1466 | -0.00158984   0.0220121 -0.00528218   0.0366967
1467 |   0.0110963 -0.00528218   0.0925277   -0.144472
1468 | -0.00526723   0.0366967   -0.144472    0.795677
1469 | x_ =   191.735
1470 |    37.567
1471 |   2.29034
1472 | -0.217098
1473 | P_ =    0.0222885 -1.04696e-05    0.0432018   0.00107376
1474 | -1.04696e-05    0.0223395    0.0010736     0.037978
1475 |    0.0432018    0.0010736     0.260342    -0.111398
1476 |   0.00107376     0.037978    -0.111398     0.802248
1477 | x_ =   191.703
1478 |   37.5604
1479 |   2.20377
1480 | -0.234389
1481 | P_ =   0.0142825 -0.00156885   0.0110421 -0.00510812
1482 | -0.00156885    0.022021 -0.00517563   0.0367512
1483 |   0.0110421 -0.00517563   0.0921718   -0.143603
1484 | -0.00510812   0.0367512   -0.143603    0.796037
1485 | x_ =   193.475
1486 |   37.2794
1487 |   1.36525
1488 | -0.334537
1489 | P_ =    0.0222885 -1.03923e-05    0.0432054    0.0010651
1490 | -1.03923e-05    0.0223395   0.00106477    0.0379744
1491 |    0.0432054   0.00106477     0.259936    -0.110407
1492 |    0.0010651    0.0379744    -0.110407     0.802656
1493 | x_ =   193.567
1494 |   37.2971
1495 |   1.86005
1496 | -0.242849
1497 | P_ =   0.0142735 -0.00154639   0.0109938 -0.00494013
1498 | -0.00154639   0.0220303 -0.00510172   0.0367997
1499 |   0.0109938 -0.00510172   0.0913655    -0.14161
1500 | -0.00494013   0.0367997    -0.14161    0.796837
1501 | x_ =  195.986
1502 |  37.3964
1503 |  2.95945
1504 | 0.365282
1505 | P_ =    0.0222884 -1.02414e-05    0.0432118   0.00104964
1506 | -1.02414e-05    0.0223395   0.00104866    0.0379681
1507 |    0.0432118   0.00104866     0.259265    -0.108741
1508 |   0.00104964    0.0379681    -0.108741     0.803327
1509 | x_ =  195.785
1510 |  37.3582
1511 |  2.07145
1512 | 0.199258
1513 | P_ =   0.0142695 -0.00153629   0.0109998 -0.00498537
1514 | -0.00153629   0.0220347 -0.00508443   0.0367946
1515 |   0.0109998 -0.00508443   0.0907974   -0.140189
1516 | -0.00498537   0.0367946   -0.140189    0.797415
1517 | x_ =  197.726
1518 |  37.7274
1519 |  1.82572
1520 | 0.481806
1521 | P_ =    0.0222884 -1.01475e-05    0.0432154   0.00104034
1522 | -1.01475e-05    0.0223396   0.00103971    0.0379644
1523 |    0.0432154   0.00103971     0.258907    -0.107844
1524 |   0.00104034    0.0379644    -0.107844     0.803691
1525 | x_ =  197.691
1526 |  37.7205
1527 |  1.79369
1528 | 0.475296
1529 | P_ =   0.0142705 -0.00153901   0.0110278 -0.00510297
1530 | -0.00153901   0.0220339 -0.00510776   0.0367671
1531 |   0.0110278 -0.00510776   0.0907096   -0.139968
1532 | -0.00510297   0.0367671   -0.139968    0.797515
1533 | x_ =  199.296
1534 |  37.2927
1535 |  1.38567
1536 | -1.06834
1537 | P_ =    0.0222884 -1.01424e-05    0.0432154    0.0010401
1538 | -1.01424e-05    0.0223396   0.00104004    0.0379643
1539 |    0.0432154   0.00104004     0.258931    -0.107905
1540 |    0.0010401    0.0379643    -0.107905     0.803675
1541 | x_ =  199.353
1542 |  37.3033
1543 |  1.68851
1544 | -1.01466
1545 | P_ =   0.0142543 -0.00149716   0.0109184 -0.00473832
1546 | -0.00149716   0.0220504 -0.00491124   0.0368757
1547 |   0.0109184 -0.00491124   0.0898796    -0.13785
1548 | -0.00473832   0.0368757    -0.13785    0.798329
1549 | x_ =  201.544
1550 |  36.9921
1551 |  2.69514
1552 | 0.203906
1553 | P_ =    0.0222883 -9.96053e-06    0.0432233    0.0010203
1554 | -9.96053e-06    0.0223397   0.00101938    0.0379565
1555 |    0.0432233   0.00101938     0.258049    -0.105649
1556 |    0.0010203    0.0379565    -0.105649     0.804549
1557 | x_ =   201.335
1558 |    36.954
1559 |   1.80514
1560 | 0.0468283
1561 | P_ =   0.0142483 -0.00148169   0.0109115 -0.00471053
1562 | -0.00148169   0.0220566 -0.00489754    0.036884
1563 |   0.0109115 -0.00489754   0.0889412   -0.135416
1564 | -0.00471053    0.036884   -0.135416    0.799269
1565 | x_ =   202.964
1566 |   36.4608
1567 |   1.44018
1568 | -0.878651
1569 | P_ =    0.0222882 -9.79399e-06    0.0432296   0.00100399
1570 | -9.79399e-06    0.0223397   0.00100276    0.0379501
1571 |    0.0432296   0.00100276     0.257419    -0.104006
1572 |   0.00100399    0.0379501    -0.104006     0.805184
1573 | x_ =   202.938
1574 |   36.4566
1575 |   1.42104
1576 | -0.881796
1577 | P_ =    0.014234 -0.00144313   0.0108519 -0.00454567
1578 | -0.00144313   0.0220712 -0.00473842   0.0369431
1579 |   0.0108519 -0.00473842   0.0879367    -0.13275
1580 | -0.00454567   0.0369431    -0.13275    0.800268
1581 | x_ =  203.994
1582 |  36.1924
1583 | 0.739444
1584 | 0.150829
1585 | P_ =    0.0222882 -9.59151e-06    0.0432376    0.0009826
1586 | -9.59151e-06    0.0223398  0.000981505    0.0379421
1587 |    0.0432376  0.000981505     0.256568    -0.101735
1588 |    0.0009826    0.0379421    -0.101735     0.806032
1589 | x_ =  204.044
1590 |  36.2015
1591 |  1.20283
1592 | 0.230669
1593 | P_ =    0.014232 -0.00143803   0.0108757 -0.00464399
1594 | -0.00143803   0.0220732 -0.00476535   0.0369205
1595 |   0.0108757 -0.00476535    0.087303   -0.131041
1596 | -0.00464399   0.0369205   -0.131041     0.80092
1597 | Accuracy - RMSE:
1598 |  0.18566
1599 | 0.190271
1600 | 0.474522
1601 | 0.811142
1602 | 


--------------------------------------------------------------------------------