# include "PCH. H" 
# include "math. H" 
# include "normalizeAngle. H" 
# include "getLandmarkCenter. H" 
# include "new_state. H" 
# include "updateNew. H" 
# include "nearby_LMs. H" 
# include "updateExisting. H" 
# include 
# include 
# include 
#include  
Using the namespace Eigen; 
using namespace std; 
# define PI 3, 14 
Eigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixEigen: : MatrixXd Intensity; 
Eigen: : MatrixXd Laser; 
Eigen: : MatrixXd xest (3, 1), A Pest, A (3, 3), W (3, 3), Q (3, 3), zlaser (2, 3); 
Eigen: : MatrixXd distance0 (2, 1); 
Int numStates, k; 
Void readdate () 
{
Size_t M, N; 
MATFile * pmatFile=NULL; 
MxArray * pMxArray=NULL; 
Double * initA; 

PmatFile=matOpen (" E://beac_juan3. Mat ", "r"); 
Estbeac pMxArray=matGetVariable (pmatFile, ""); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
The landmarks (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
PMxArray=matGetVariable (pmatFile, "Index"); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
Index1 (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
PMxArray=matGetVariable (pmatFile, "Sensor"); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
Sensor (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
PMxArray=matGetVariable (pmatFile, "Time"); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
The Time (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
TimeLaser pMxArray=matGetVariable (pmatFile, ""); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
TimeLaser (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
PMxArray=matGetVariable (pmatFile, "Velocity"); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
Velocity (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
PMxArray=matGetVariable (pmatFile, "Steering"); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
Steering (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
Time_VS pMxArray=matGetVariable (pmatFile, ""); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
Time_VS (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
GPSLat pMxArray=matGetVariable (pmatFile, ""); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
GPSLat (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
GPSLon pMxArray=matGetVariable (pmatFile, ""); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
GPSLon (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
TimeGPS pMxArray=matGetVariable (pmatFile, ""); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
for (int i=0; I & lt; M; I++) 
For (int j=0; J & lt; N; J++) 
{
TimeGPS (I, j)=initA [M + I * j]; 
} 
PmatFile=matOpen (" E://data_set ", "r"); 
PMxArray=matGetVariable (pmatFile, "Intensity"); 
InitA=(double *) mxGetData (pMxArray); 
M=mxGetM (pMxArray); 
N=mxGetN (pMxArray); 
nullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnullnull