#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<mat.h>
#include<Core>
#include<Dense>
#include <iostream>
using namespace Eigen;
using namespace std;
#define PI 3,14
Eigen::Matrix<double, 15, 2> landmarks;
Eigen::Matrix<double, 1, 6213> Index1;
Eigen::Matrix<double, 1, 6213> Sensor;
Eigen::Matrix<double, 1, 6213> Time;
Eigen::Matrix<double, 1, 586> TimeLaser;
Eigen::Matrix<double, 1, 5001> Velocity;
Eigen::Matrix<double, 1, 5001> Steering;
Eigen::Matrix<double, 1, 5001> Time_VS;
Eigen::Matrix<double, 1, 626> GPSLat;
Eigen::Matrix<double, 1, 626> GPSLon;
Eigen::Matrix<double, 1, 626> TimeGPS;
Eigen::MatrixXd Intensity;
Eigen::MatrixXd Laser;
Eigen::MatrixXd xest(3, 1),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");
pMxArray = matGetVariable(pmatFile, "estbeac");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
landmarks(i,j) = initA[M*j + i];
}
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 < M; i++)
for (int j = 0; j < N; j++)
{
Index1(i,j) = initA[M*j + i];
}
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 < M; i++)
for (int j = 0; j < N; j++)
{
Sensor(i,j) = initA[M*j + i];
}
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 < M; i++)
for (int j = 0; j < N; j++)
{
Time(i,j) = initA[M*j + i];
}
pmatFile = matOpen("E://data_set", "r");
pMxArray = matGetVariable(pmatFile, "TimeLaser");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
TimeLaser(i,j) = initA[M*j + i];
}
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 < M; i++)
for (int j = 0; j < N; j++)
{
Velocity(i,j) = initA[M*j + i];
}
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 < M; i++)
for (int j = 0; j < N; j++)
{
Steering(i,j) = initA[M*j + i];
}
pmatFile = matOpen("E://data_set", "r");
pMxArray = matGetVariable(pmatFile, "Time_VS");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
Time_VS(i,j) = initA[M*j + i];
}
pmatFile = matOpen("E://data_set", "r");
pMxArray = matGetVariable(pmatFile, "GPSLat");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
GPSLat(i,j) = initA[M*j + i];
}
pmatFile = matOpen("E://data_set", "r");
pMxArray = matGetVariable(pmatFile, "GPSLon");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
GPSLon(i,j) = initA[M*j + i];
}
pmatFile = matOpen("E://data_set", "r");
pMxArray = matGetVariable(pmatFile, "TimeGPS");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
TimeGPS(i,j) = initA[M*j + i];
}
pmatFile = matOpen("E://data_set", "r");
pMxArray = matGetVariable(pmatFile, "Intensity");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
Intensity(i,j) = initA[M*j + i];
}
pmatFile = matOpen("E://data_set", "r");
pMxArray = matGetVariable(pmatFile, "Laser");
initA = (double*)mxGetData(pMxArray);
M = mxGetM(pMxArray);
N = mxGetN(pMxArray);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
{
Laser(i,j) = initA[M*j + i];
}
}
其他的函式檔案就不放了，求大佬看看。
uj5u.com熱心網友回復：
崩潰的時候在彈出的對話框按相應按鈕進入除錯，按Alt+7鍵查看Call Stack即“呼叫堆疊”里面從上到下列出的對應從里層到外層的函式呼叫歷史。雙擊某一行可將游標定位到此次呼叫的源代碼或匯編指令處，看不懂時雙擊下一行，直到能看懂為止。uj5u.com熱心網友回復：
請問解決了嗎？轉載請註明出處，本文鏈接：https://www.uj5u.com/houduan/44249.html
標籤：C++ 語言
上一篇：求這道題的c++的代碼。
下一篇：51單片機控制LCD1602模塊