460 lines
16 KiB
C++
460 lines
16 KiB
C++
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||
* File Name : pdr_main.cpp
|
||
* Department : Sensor Algorithm Team
|
||
* Current Version : V2.0
|
||
* Author : logzhan
|
||
* Date of Issued : 2021.1.28
|
||
* Comments : PDR PC端仿真主文件
|
||
* Modify : 2021/02/04 logzhan : 优化kml显示，kml增加航向角图形
|
||
* 显示，hdop、heading、accuracy信息在Google Map的显示，方便调
|
||
* 试代码。
|
||
* 2021/02/18 logzhan : 增加kml的时间显示，能够拖动进
|
||
* 度条显示轨迹。可以对比查看轨迹效果。
|
||
*
|
||
* 一、优化方向和工作内容
|
||
* 1) GPS速度实际需要乘以0.51444的，但是除了滤波器计算步频之外，
|
||
* 其他地方都是用的实际的GPS速度，包括步数预测
|
||
* 2) 惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
|
||
* 时间分析发现，buffer mean耗费了大量时间
|
||
* 2021/02/10 logzhan:
|
||
* 1) 需要跟品质谈高通对比版本，防止最后效果和高通SAP5.0持平后，
|
||
* 高通出了新版本，品质不认可效果改进。
|
||
* 2) 目前GPS的速度是没有乘以0.51444的，许多地方计算没有统一，需
|
||
* 要统一。
|
||
* 3）惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
|
||
* 时间分析发现，buffer mean耗费了大量时间
|
||
* 4)*目前在不依赖磁力计的版本也能达到比以前有磁力计效果还好。但是
|
||
* 磁力计依然是一个重要的辅助确定方向的手段。所以利用GPS航向作为
|
||
* 观测，磁力计启动动态校准，校准完成后才启动辅助磁力计校准方向，
|
||
* 或者作为一个观测量，用于提升预测方向的准确程度。
|
||
* 5)*动态偏移对准，后续会开始开发PCA方向估计，那么PCA估计方向肯定
|
||
* 和真实方向存在一定的偏差，需要动态对准偏移。
|
||
* 6）目前的计步器，输出不够实时，对于步长的估计不够准确。目前测试
|
||
* 的情况是，加入动态步长估计后貌似轨迹会偏大，是不是计步偏少？
|
||
* 7）这个步频计算用到了fft，感觉没有太大作用，可以考虑在新版本中
|
||
* 去除，然后以后开发时如果需要可以参考原理重新优化。
|
||
* 8) 赖工提到路网信息，目前比较确定的是能拿到路的方向信息，考虑
|
||
* 在选定的场景标定一下经纬度和最近的道路方向信息，观察如何提升
|
||
* 定位精度(方向辅助，减少方向上的漂移)。
|
||
* 9) PDR 目前一定概率初始对准方向会偏，所以初始对准时，检测是否
|
||
* 是直线路径，然后考虑用轨迹计算平均方向角，辅助初始化。
|
||
* 10) 跟张经睿讨论过，场景识别模块以前主要是针对以前的单频手机，
|
||
* 所以这个模块考虑重构，因为以前单频手机的参数和现在双频手机不同。
|
||
* 需要重新优化。
|
||
* 11) 目前PDR算法在观测方向时，还是一定概率会被带偏，所以导致转弯
|
||
* 转向不够，然而又不够相信GPS方向，考虑通过多次GPS方向角方差不大时
|
||
* 调整GPS方向角的噪声策略。
|
||
* 12) kml支持时间信息，可以拖动进度条比较轨迹同时运行的情况 ---- logzhan finish
|
||
*
|
||
* 二、调整规划为：
|
||
第一期：手持稳定优化 + 摆手(平滑模块主要生效) + 多圈优化 => 不会比之前效果差，部分场景提升
|
||
第二期：手持稳定优化 + 摆手(PCA + 平滑) + 多圈 => 在摆手场景优化，覆盖一大半场景
|
||
第三期：手持稳定优化 + 摆手 + 任意动作 => 鲁棒性优化，覆盖全部使用场景
|
||
第四期：手持稳定优化 + 摆手 + 任意动作 + 多圈 + 真值仪 => 保证轨迹优化和方向优化条件下，精度提升，可以方便对外
|
||
********************************************************************************/
|
||
#include <stdio.h>
|
||
#include <string>
|
||
#include <errno.h>
|
||
#include <stdlib.h>
|
||
#include "pdr_api.h"
|
||
#include "Main.h"
|
||
#include "Utils.h"
|
||
#include "time.h"
|
||
using namespace std;
|
||
|
||
/* Global Variable Definition ------------------------------------------------*/
|
||
ResultTracks resTracks;
|
||
FILE* SimFile = NULL;
|
||
/* Extern Variable Definition ------------------------------------------------*/
|
||
extern "C" PDR Pdr;
|
||
extern "C" double refGpsYaw;
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : main
|
||
* Description : pdr算法仿真用的main函数
|
||
* Date : 2021/01/25 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
int main(int argc, char* argv[])
|
||
{
|
||
|
||
string logPath = "E:\\SoftwareProject\\GnssIns\\PDR_Data\\Datasets\\";
|
||
string logDate = "shenzhen";
|
||
string logLabel = "data\\";
|
||
string fileHead;
|
||
char line[256] = { 0 };
|
||
|
||
// 解析命令行, 用于解析参数
|
||
for (int i = 0; i < argc; i++) {
|
||
string s = argv[i];
|
||
if(i == 1)logPath = s;
|
||
if(i == 2)logDate = s;
|
||
}
|
||
|
||
clock_t t = clock();
|
||
// 获取kml输出路径
|
||
string kmlPath = logPath + logDate + "\\output\\";
|
||
// 仿真文件夹路径和Catalog.txt路径
|
||
string filePath = logPath + logDate + "\\" + logLabel + "\\";
|
||
string catalogPath = filePath + "catalog.txt";
|
||
|
||
FILE* catalogFp = fopen(catalogPath.c_str(), "r");
|
||
if (catalogFp == NULL){
|
||
printf("open catalog.txt file %s error %s\n", catalogPath.c_str(),
|
||
strerror(errno));
|
||
system("pause");
|
||
return -1;
|
||
}
|
||
|
||
while (!feof(catalogFp))
|
||
{
|
||
FILE* fileFp = getSimulateFile(catalogFp, filePath, fileHead);
|
||
// PDR 仿真流程
|
||
Algorithm_Init();
|
||
|
||
LctFs_t locFusion;
|
||
memset(&locFusion,0, sizeof(locFusion));
|
||
memset(&resTracks, 0, sizeof(resTracks));
|
||
|
||
// 解析文本仿真
|
||
while (fgets(line, 256, fileFp)){
|
||
// 解析文本数据仿真
|
||
if (ParseDataAndUpdate(line, &locFusion) != TYPE_FIX_NONE){
|
||
// 更新结果轨迹，用于最后的kml生成
|
||
UpdateResTrack(resTracks, locFusion);
|
||
}
|
||
}
|
||
// 关闭PDR算法，释放资源
|
||
Algorithm_DeInit();
|
||
// 写结果kml
|
||
KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1c"));
|
||
// 关闭文件
|
||
fclose(fileFp);
|
||
}
|
||
// 打印仿真时间信息
|
||
printf("PDR simulate spends %f s\n", ((double)(clock() - t) / CLOCKS_PER_SEC));
|
||
system("pause");
|
||
return 0;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : getSimulateFile
|
||
* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针
|
||
* catalogFp : 目录文件的文件指针
|
||
* path_file ：仿真文件的文件目录
|
||
* fileHead : 通过C++引用返回的去除了扩展名的文件名称
|
||
* Date : 2021/01/25 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead) {
|
||
|
||
char file_name[256] = { 0 };
|
||
FILE* fp = NULL;
|
||
|
||
if (fscanf(catalogFp, "%s\n", file_name) == -1){
|
||
printf("get data file name failed\n");
|
||
system("pause");
|
||
exit(-1);
|
||
}
|
||
string fileNamePath = path_file + string(file_name);
|
||
fp = fopen(fileNamePath.c_str(), "rb");
|
||
if (fp == NULL){
|
||
printf("open file %s failed error %s\n", fileNamePath.c_str(), strerror(errno));
|
||
system("pause");
|
||
exit(-1);
|
||
}
|
||
// 去除文件扩展名.csv
|
||
fileHead = file_name;
|
||
fileHead = fileHead.substr(0, fileHead.length() - 4);
|
||
|
||
printf("%s\n", fileNamePath.c_str());
|
||
return fp;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : gpsYaw2GoogleYaw
|
||
* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换
|
||
* 为谷歌支持的Yaw规则
|
||
* Date : 2021/01/25 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
double gpsYaw2GoogleYaw(double heading) {
|
||
double gh = R2D(heading) - 180;
|
||
while (gh > 360)gh -= 360;
|
||
while (gh < 0)gh += 360;
|
||
return gh;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : UpdateResTrack
|
||
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs)
|
||
{
|
||
resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude;
|
||
resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;
|
||
resTrack.pdrTrack[resTrack.pdrLen].heading =
|
||
gpsYaw2GoogleYaw(lctfs.pdrHeading);
|
||
resTrack.pdrTrack[resTrack.pdrLen].hdop = lctfs.hdop;
|
||
resTrack.pdrTrack[resTrack.pdrLen].accuracy = lctfs.accuracy;
|
||
resTrack.pdrTrack[resTrack.pdrLen].time = lctfs.time;
|
||
resTrack.pdrTrack[resTrack.pdrLen].motionType = lctfs.motionType;
|
||
resTrack.pdrLen++;
|
||
|
||
if (lctfs.gpsLat > 0 && lctfs.gpsLon > 0) {
|
||
resTrack.gpsTrack[resTrack.gpsLen].lat = lctfs.gpsLat;
|
||
resTrack.gpsTrack[resTrack.gpsLen].lon = lctfs.gpsLon;
|
||
resTrack.gpsTrack[resTrack.gpsLen].heading =
|
||
gpsYaw2GoogleYaw(lctfs.gpsHeading);
|
||
|
||
resTrack.gpsTrack[resTrack.gpsLen].vel = lctfs.gpsSpeed;
|
||
resTrack.gpsTrack[resTrack.gpsLen].hdop = lctfs.hdop;
|
||
resTrack.gpsTrack[resTrack.gpsLen].accuracy = lctfs.accuracy;
|
||
resTrack.gpsTrack[resTrack.gpsLen].time = lctfs.time;
|
||
resTrack.gpsLen++;
|
||
}
|
||
}
|
||
|
||
string time2str(double t) {
|
||
long time = (long)t;
|
||
if (t < 10) {
|
||
string str = "0" + std::to_string(time);
|
||
return str;
|
||
}else {
|
||
string str = std::to_string(time);
|
||
return str;
|
||
}
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : KmlWrite
|
||
* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式
|
||
* path : kml文件的输出文件路径
|
||
* name : kml文件主体名称
|
||
* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本
|
||
* Date : 2020/11/1 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void KmlWrite(string path, string name, string postfix)
|
||
{
|
||
string pdrColor = "ff0000ff";
|
||
string gpsColor = "ff00ffff";
|
||
|
||
if (path == "") {
|
||
return;
|
||
}
|
||
|
||
string kmlPath = path + name + postfix + ".kml";
|
||
string kmlName = name + postfix;
|
||
FILE* fid = fopen(kmlPath.c_str(), "w");
|
||
|
||
fprintf(fid, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
|
||
fprintf(fid, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
|
||
fprintf(fid, "\t<Document>\n");
|
||
fprintf(fid, "\t\t<name>%s</name>\n", kmlName.c_str());
|
||
|
||
fprintf(fid, "\t\t<Folder id=\"ID1\">\n");
|
||
fprintf(fid, "\t\t<name>GPS Fixs</name>\n");
|
||
|
||
// 写gps结果
|
||
for (int i = 0; i < resTracks.gpsLen; i++) {
|
||
fprintf(fid, "\t\t<Placemark>\n");
|
||
fprintf(fid, "\t\t\t<Style>\n");
|
||
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||
|
||
// 描述轨迹方向信息
|
||
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", resTracks.gpsTrack[i].heading);
|
||
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", gpsColor.c_str());
|
||
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||
fprintf(fid, "\t\t\t</Style>\n");
|
||
|
||
// 写入时间信息
|
||
double h, m, s;
|
||
pdr_utc2hms(resTracks.gpsTrack[i].time, &h, &m, &s);
|
||
fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||
time2str(m).c_str(), time2str(s).c_str());
|
||
fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||
|
||
// 写入描述信息
|
||
fprintf(fid, "\t\t\t<description>\n");
|
||
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||
fprintf(fid, "\t\t\t\t<dl>\n");
|
||
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||
resTracks.gpsTrack[i].hdop);
|
||
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m)</dd>\n",
|
||
resTracks.gpsTrack[i].accuracy);
|
||
fprintf(fid, "\t\t\t\t<dd>speed : %.2f (m/s)</dd>\n",
|
||
resTracks.gpsTrack[i].vel);
|
||
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||
resTracks.gpsTrack[i].heading);
|
||
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||
fprintf(fid, "\t\t\t</description>\n");
|
||
// 写入坐标信息
|
||
fprintf(fid, "\t\t\t<Point>\n");
|
||
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||
resTracks.gpsTrack[i].lon,
|
||
resTracks.gpsTrack[i].lat);
|
||
fprintf(fid, "\t\t\t</Point>\n");
|
||
fprintf(fid, "\t\t</Placemark>\n");
|
||
}
|
||
fprintf(fid, "\t\t</Folder>\n");
|
||
|
||
fprintf(fid, "\t\t<Folder id=\"ID2\">\n");
|
||
fprintf(fid, "\t\t<name>PDR Fixs</name>\n");
|
||
// 写pdr结果
|
||
for (int i = 0; i < resTracks.pdrLen; i++) {
|
||
fprintf(fid, "\t\t<Placemark>\n");
|
||
fprintf(fid, "\t\t\t<Style>\n");
|
||
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n",
|
||
resTracks.pdrTrack[i].heading);
|
||
|
||
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", pdrColor.c_str());
|
||
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||
fprintf(fid, "\t\t\t</Style>\n");
|
||
|
||
// 写入时间信息
|
||
double h, m, s;
|
||
pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s);
|
||
fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||
time2str(m).c_str(), time2str(s).c_str());
|
||
fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||
|
||
// description
|
||
fprintf(fid, "\t\t\t<description>\n");
|
||
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||
fprintf(fid, "\t\t\t\t<dl>\n");
|
||
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||
resTracks.pdrTrack[i].hdop);
|
||
|
||
fprintf(fid, "\t\t\t\t<dd>MOTION TYPE : %s (m/s)</dd>\n",
|
||
Motion2TypeStr(resTracks.pdrTrack[i].motionType));
|
||
|
||
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m/s)</dd>\n",
|
||
resTracks.pdrTrack[i].accuracy);
|
||
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||
resTracks.pdrTrack[i].heading);
|
||
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||
fprintf(fid, "\t\t\t</description>\n");
|
||
|
||
fprintf(fid, "\t\t\t<Point>\n");
|
||
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||
resTracks.pdrTrack[i].lon,
|
||
resTracks.pdrTrack[i].lat);
|
||
fprintf(fid, "\t\t\t</Point>\n");
|
||
fprintf(fid, "\t\t</Placemark>\n");
|
||
}
|
||
fprintf(fid, "\t\t</Folder>\n");
|
||
|
||
fprintf(fid, "\t</Document>\n");
|
||
fprintf(fid, "</kml>\n");
|
||
|
||
fclose(fid);
|
||
}
|
||
|
||
|
||
#ifdef _WIN32
|
||
/**----------------------------------------------------------------------
|
||
* Function : PDR_Init
|
||
* Description : 初始化PDR算法(DLL调用)
|
||
* Date : 2022/09/15 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void LibPDR_Init() {
|
||
Algorithm_Init();
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : PDR_StepUpdateGPS
|
||
* Description : 仿真器按照GPS单步执行(DLL调用)
|
||
* Date : 2022/9/16 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
PosFusion LibPDR_StepUpdateGPS(int useGpsFlg)
|
||
{
|
||
PosFusion fusion;
|
||
fusion.vaild = 0;
|
||
|
||
Nmea_t location_nmea;
|
||
Nmea_t* ln = &location_nmea;
|
||
imu imu_data;
|
||
LctFs_t lct_fusion;
|
||
memset(&lct_fusion, 0, sizeof(lct_fusion));
|
||
//memset(&location_nmea, 0, sizeof(location_nmea));
|
||
memset(&imu_data, 0, sizeof(imu_data));
|
||
/*memset(&kf, 0, sizeof(kf));*/
|
||
char line[256] = { 0 };
|
||
while (fgets(line, 256, SimFile)) {
|
||
|
||
int flag = ParseDataAndUpdate(line, &lct_fusion);
|
||
|
||
if (flag) {
|
||
|
||
fusion.gpsLat = lct_fusion.gpsLat;
|
||
fusion.gpsLon = lct_fusion.gpsLon;
|
||
}
|
||
|
||
// 写PDR融合结果
|
||
if (flag) {
|
||
fusion.lat = lct_fusion.latitude;
|
||
fusion.lon = lct_fusion.longitudinal;
|
||
|
||
fusion.t = lct_fusion.time;
|
||
fusion.vaild = 1;
|
||
|
||
break;
|
||
}
|
||
}
|
||
return fusion;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : setSimulatorFileCsvPath
|
||
* Description : 设置仿真文件的路径(DLL调用)
|
||
* Date : 2020/8/3 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void setSimulatorFileCsvPath(char* path) {
|
||
if (SimFile == NULL) {
|
||
SimFile = fopen(path, "rb");
|
||
}
|
||
else {
|
||
fclose(SimFile);
|
||
SimFile = NULL;
|
||
}
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : setRefGpsYaw
|
||
* Description : 设置GPS参考角度
|
||
* Date : 2020/8/3 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void setRefGpsYaw() {
|
||
refGpsYaw = Pdr.gpsHeading;
|
||
printf("ref Gps Yaw = %f\n", refGpsYaw);
|
||
//g_pdr.sysStatus = IS_INITIAL;
|
||
//g_pdr.fusionPdrFlg = ON;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : closePdrAlgo
|
||
* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用)
|
||
* Date : 2020/8/3 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void LibPDR_DeInit() {
|
||
Algorithm_DeInit();
|
||
|
||
if (SimFile != NULL) {
|
||
fclose(SimFile);
|
||
SimFile = NULL;
|
||
}
|
||
}
|
||
#endif |