[
{
"path": ".gitignore",
"content": "# Created by .ignore support plugin (hsz.mobi)\n### JetBrains template\n# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and WebStorm\n# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839\n\n# User-specific stuff\n.idea/**/workspace.xml\n.idea/**/tasks.xml\n.idea/**/usage.statistics.xml\n.idea/**/dictionaries\n.idea/**/shelf\n\n# Sensitive or high-churn files\n.idea/**/dataSources/\n.idea/**/dataSources.ids\n.idea/**/dataSources.local.xml\n.idea/**/sqlDataSources.xml\n.idea/**/dynamic.xml\n.idea/**/uiDesigner.xml\n.idea/**/dbnavigator.xml\n\n# Gradle\n.idea/**/gradle.xml\n.idea/**/libraries\n\n# Gradle and Maven with auto-import\n# When using Gradle or Maven with auto-import, you should exclude module files,\n# since they will be recreated, and may cause churn. Uncomment if using\n# auto-import.\n# .idea/modules.xml\n# .idea/*.iml\n# .idea/modules\n\n# CMake\ncmake-build-*/\n\n# Mongo Explorer plugin\n.idea/**/mongoSettings.xml\n\n# File-based project format\n*.iws\n\n# IntelliJ\nout/\n\n# mpeltonen/sbt-idea plugin\n.idea_modules/\n\n# JIRA plugin\natlassian-ide-plugin.xml\n\n# Cursive Clojure plugin\n.idea/replstate.xml\n\n# Crashlytics plugin (for Android Studio and IntelliJ)\ncom_crashlytics_export_strings.xml\ncrashlytics.properties\ncrashlytics-build.properties\nfabric.properties\n\n# Editor-based Rest Client\n.idea/httpRequests\n.idea"
},
{
"path": ".travis.yml",
"content": "language: cpp\n\ncompiler:\n - gcc\n\nscript:\n - mkdir build\n - cd build\n - cmake .. && make\n - ./Location"
},
{
"path": "CMakeLists.txt",
"content": "cmake_minimum_required(VERSION 3.9)\nproject(Location)\nset(CMAKE_CXX_STANDARD 11)\n\nset(INC_DIR ./include/eigen3/)\ninclude_directories(${INC_DIR})\n\ninclude_directories(${PROJECT_SOURCE_DIR}/math)\nadd_subdirectory(math)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/models)\nadd_subdirectory(models)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/location)\nadd_subdirectory(location)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/sensor)\nadd_subdirectory(sensor)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/system)\nadd_subdirectory(system)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/utils)\nadd_subdirectory(utils)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/test)\nadd_subdirectory(test)\n\n#add_library(lib ./math/KalmanFilter.h ./math/KalmanFilter.cpp ./models/AHRS.cpp ./sensor/Accelerometer.cpp ./sensor/Gyroscope.cpp ./sensor/GPS.cpp ./sensor/Magnetometer.cpp ./math/Quaternions.cpp math/Optimizer.cpp sensor/Sensor.cpp sensor/Sensor.h location/Location.cpp system/Status.cpp ./include/googletest/ test/utils/DataFormat.cpp test/utils/DataFormat.h test/TestLocation.cpp test/TestCalibration.cpp test/TestCalibration.h math/LPF.cpp math/LPF.h models/StrapdownAHRS.cpp config/Config.cpp config/Config.h sensor/Gravity.cpp sensor/Gravity.h sensor/Compass.cpp sensor/Compass.h)\n#link_libraries(lib)\n#add_executable(Location main.cpp ${lib})\nadd_executable(Location main.cpp)\n\ntarget_link_libraries(Location Location_math)\ntarget_link_libraries(Location Location_models)\ntarget_link_libraries(Location Location_location)\ntarget_link_libraries(Location Location_sensor)\ntarget_link_libraries(Location Location_system)\ntarget_link_libraries(Location Location_utils)\ntarget_link_libraries(Location Location_test)"
},
{
"path": "README.md",
"content": "# LOCATION\n\n[](https://travis-ci.org/yyccR/Location)\n\n> Positioning is the most basic and crucial step in the driving navigation. An accurate positioning can effectively improve the accuracy of the road-binding, and can also sense the change of the driving pattern more accurately. Since the project is mainly based on mobile phones for navigation and positioning, Currently used is the built-in sensor data (gyroscope, accelerometer, geomagnetic meter, direction sensor, gravity sensor) and GPS data fusion positioning.\n\n## Sensor data required.\n\n- [X] gyroscope(x, y, z).\n- [X] accelerometer(x, y, z).\n- [X] geomagnetic meter(x, y, z).\n- [X] gravity sensor(x, y, z).\n- [X] direction sensor(roll, pitch, yaw).\n- [X] compass(degree).\n- [X] road info(distance to next cross, bearing, road type).\n- [X] GPS(lng, lat, alt, accuracy, speed, bearing, t).\n\n\n## Some implement details\n\n- sensor data filter.\n\n![](\"https://raw.githubusercontent.com/yyccR/Pictures/master/INS/ornt_filter.png\")
\n\n- GPS fusion INS under uncoupling system.\n\n![](\"https://raw.githubusercontent.com/yyccR/Pictures/master/INS/origin)
![](\"https://raw.githubusercontent.com/yyccR/Pictures/master/INS/final)
\n\n## Quick start\nFirst make sure gcc and cmake was installed, and include this library into your project.\n\n```\ngit clone https://github.com/yyccR/Location.git\n```\n\nSecond open your `CMakeLists.txt` and add these:\n\n```\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/include/eigen3)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/math)\nadd_subdirectory(Location/math)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/models)\nadd_subdirectory(Location/models)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/location)\nadd_subdirectory(Location/location)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/sensor)\nadd_subdirectory(Location/sensor)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/system)\nadd_subdirectory(Location/system)\n\ntarget_link_libraries(${PROJECT_NAME} Location_math)\ntarget_link_libraries(${PROJECT_NAME} Location_models)\ntarget_link_libraries(${PROJECT_NAME} Location_location)\ntarget_link_libraries(${PROJECT_NAME} Location_sensor)\ntarget_link_libraries(${PROJECT_NAME} Location_system)\ntarget_link_libraries(${PROJECT_NAME} Location_test)\n```\n\nfinal open your main file, and add the test code.\n\n```\n#include \n#include \n#include \"sensor/GPS.h\"\n#include \"location/Location.h\"\n\nusing namespace Eigen;\nusing namespace std;\n\nint main() {\n\n Location location;\n Vector3d gyro_data_v(0.004263,0.019169,-0.001014);\n Vector3d mag_data_v(-2.313675,-82.446960,-366.183838);\n Vector3d acc_data_v(0.105081,0.108075,9.774973);\n VectorXd gps_data_v(7);\n gps_data_v << 114.174118,22.283789,0.0,0.0,24.0,0.0,1554348968704.665039;\n Vector3d g_data_v(0.094139, 0.107857,9.808955);\n Vector3d ornt_data_v(-0.549866,0.629957,-0.069398);\n Vector3d road_data(1000.0, 0.0, 0);\n location.PredictCurrentPosition(gyro_data_v,acc_data_v,mag_data_v,gps_data_v,g_data_v,ornt_data_v, road_data);\n cout << location.GetGNSSINS().lng << \" \" << location.GetGNSSINS().lat << endl;\n return 0;\n}\n```\n\nif you see the output `114.174 22.2838` that means this library was embedded to your project successfully.\n\n## Input data format.\n\n- gyroscope(x, y, z), origin gyroscope data, unit rad/s\n\n- accelerometer(x, y, z), origin accelerometer data, unit m/s²\n\n- geomagnetic meter(x, y, z), origin geomagnetic data, unit μt\n\n- gravity sensor(x, y, z), origin gravity data, unit m/s²\n\n- direction sensor(roll, pitch, yaw), origin sensor data, unit degree\n\nNote that direction sensor doesn't exit actually , the 'sensor data' is computation result from system underlying algorithm.\n\n- compass(degree), origin sensor data, unit degree\n\n- road info(distance to next cross, bearing, road type)\n\nThis data is from map data, and if you couldn't search map server data, just fill in all zero `(0.0, 0.0, 0.0)`\n\n- GPS(lng, lat, alt, accuracy, speed, bearing, t)\n - lng, longitude, double\n - lat, latitude, double\n - alt, altitude, double\n - accuracy, double\n - speed, double\n - bearing, double, unit degree\n - t, timestampe, unit millisecond\n\n## More detail tutorial.\n\n- [Api calls details](docs/apiCallDetails.md)\n- [Sensor data checking](docs/SensorDataChecking.md)\n- [Impelement details](docs/implementDetails.md)\n- [Sensor calibration](docs/SensorCalibration.md)\n- [Training Stop detection model](docs/trainingStopDetectModel.md)\n\n## TODO\n\n- [X] improve CMake.\n- [X] Clean the garbage code.\n- [ ] Template processing.\n- [X] Using smart pointer instead.\n- [X] Complete all kinds of documents.\n- [X] Add quick start.\n- [ ] Add more test case.\n- [ ] Design a suitable pattern.\n\n## reference:\n\n1. 《惯性导航》秦永元\n2. 《捷联惯性导航技术(第2版 译本)》译者:张天光/王秀萍/王丽霞 作者:DavidH.Titte\n3. [An efficient orientation filter for inertial and\n inertial/magnetic sensor arrays](http://x-io.co.uk/res/doc/madgwick_internal_report.pdf)\n4. [Estimation of IMU and MARG orientation using a gradient descent algorithm](http://vigir.missouri.edu/~gdesouza/Research/Conference_CDs/RehabWeekZ%C3%BCrich/icorr/papers/Madgwick_Estimation%20of%20IMU%20and%20MARG%20orientation%20using%20a%20gradient%20descent%20algorithm_ICORR2011.pdf)\n5. [Direction Cosine Matrix IMU Theory](https://www.researchgate.net/publication/265755808_Direction_Cosine_Matrix_IMU_Theory)\n6. [METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS](http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf)\n7. [A Calibration Algorithm for Microelectromechanical Systems Accelerometers in Inertial Navigation Sensors](https://arxiv.org/pdf/1309.5075.pdf)\n8. [A Calibration Method of Three-axis Magnetic Sensor Based on Ellipsoid Fitting](https://www.researchgate.net/publication/273845104_A_Calibration_Method_of_Three-axis_Magnetic_Sensor_Based_on_Ellipsoid_Fitting)\n9. [Accuracy Improvement of Low Cost INS/GPS for Land Applications](https://prism.ucalgary.ca/bitstream/handle/1880/41142/2001_Shin.pdf?sequence=1)\n10. [Trajectory preprocessing: Computing with Spatial Trajectories](https://books.google.com.hk/books?hl=zh-CN&lr=&id=JShQJF23xBgC&oi=fnd&pg=PR3&dq=Trajectory+preprocessing.+Computing+with+Spatial+Trajectories&ots=6NUeew5i9_&sig=o7XM_QcuUnmOv5KNeezTN4H8PMw&redir_esc=y&hl=zh-CN&sourceid=cndr#v=onepage&q=Trajectory%20preprocessing.%20Computing%20with%20Spatial%20Trajectories&f=false)"
},
{
"path": "README_CN.md",
"content": "# Location\n\n> 定位是驾驶导航过程中是最基础的一步，也是十分关键的一步，一个准确的定位可以有效提高绑路的精度，也能更加精准感知驾驶形态的变化，由于本项目主要基于手机做导航定位，目前采用的是手机内置的传感器数据（陀螺仪，加速计，地磁计）以及GPS数据融合定位。\n\n## 项目所需的传感器.\n\n- [X] 陀螺仪(x, y, z).\n- [X] 加速计(x, y, z).\n- [X] 地磁计(x, y, z).\n- [X] 重力感应器(x, y, z).\n- [X] 方向传感器(roll, pitch, yaw).\n- [X] 指南针(degree).\n- [X] 道路信息(距下个路口的距离, 道路方向, 道路类型).\n- [X] GPS(经度, 纬度, 海拔, 精度, 速度, 方向, 时间戳).\n\n\n## 一些实现细节\n\n- 传感器噪声过滤与修正.\n\n\n\n- 基于非耦合的GPS融合INS.\n\n \n\n## 快速开始\n确保安装了gcc和cmake, 下载本项目到你的项目下\n\n```\ngit clone https://github.com/yyccR/Location.git\n```\n\n在项目根目录下新建`CMakeLists.txt`, 同时添加如下:\n\n```\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/include/eigen3)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/math)\nadd_subdirectory(Location/math)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/models)\nadd_subdirectory(Location/models)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/location)\nadd_subdirectory(Location/location)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/sensor)\nadd_subdirectory(Location/sensor)\n\ninclude_directories(${PROJECT_SOURCE_DIR}/Location/system)\nadd_subdirectory(Location/system)\n\ntarget_link_libraries(${PROJECT_NAME} Location_math)\ntarget_link_libraries(${PROJECT_NAME} Location_models)\ntarget_link_libraries(${PROJECT_NAME} Location_location)\ntarget_link_libraries(${PROJECT_NAME} Location_sensor)\ntarget_link_libraries(${PROJECT_NAME} Location_system)\ntarget_link_libraries(${PROJECT_NAME} Location_test)\n```\n\n在main文件里添加如下测试代码.\n\n```\n#include \n#include \n#include \"sensor/GPS.h\"\n#include \"location/Location.h\"\n\nusing namespace Eigen;\nusing namespace std;\n\nint main() {\n\n Location location;\n Vector3d gyro_data_v(0.004263,0.019169,-0.001014);\n Vector3d mag_data_v(-2.313675,-82.446960,-366.183838);\n Vector3d acc_data_v(0.105081,0.108075,9.774973);\n VectorXd gps_data_v(7);\n gps_data_v << 114.174118,22.283789,0.0,0.0,24.0,0.0,1554348968704.665039;\n Vector3d g_data_v(0.094139, 0.107857,9.808955);\n Vector3d ornt_data_v(-0.549866,0.629957,-0.069398);\n Vector3d road_data(1000.0, 0.0, 0);\n location.PredictCurrentPosition(gyro_data_v,acc_data_v,mag_data_v,gps_data_v,g_data_v,ornt_data_v, road_data);\n cout << location.GetGNSSINS().lng << \" \" << location.GetGNSSINS().lat << endl;\n return 0;\n}\n```\n\n如果输出 `114.174 22.2838` 表示已经成功内嵌了本项目.\n\n## 数据格式.\n\n- 陀螺仪(x, y, z), 单位 rad/s\n\n- 加速计(x, y, z), 单位 m/s²\n\n- 地磁计(x, y, z), 单位 μt\n\n- 重力感应器(x, y, z), 单位 m/s²\n\n- 方向传感器(roll, pitch, yaw), 单位 角度(degree)\n\n手机并没有方向传感器, 这个所谓的传感器数据是手机底层算法计算得到的。\n\n- 指南针(degree), 单位 角度(degree)\n\n- 道路信息(距离下个路口距离, 当前位置道路方向, 道路类型编码)\n\n如果拿不到道路数据, 则全部填0即可, `(0.0, 0.0, 0.0)`\n\n- GPS(lng, lat, alt, accuracy, speed, bearing, t)\n - lng, 经度, double\n - lat, 纬度, double\n - alt, 海拔, double\n - accuracy, 精度, double\n - speed, 速度, double\n - bearing, 方向, double, 单位 角度(degree)\n - t, 时间戳, 单位 毫秒(millisecond)\n\n## 更加详细的调用细节\n\n详见 docs/apiCallDetails.md\n\n## TODO\n\n- [X] CMakeLists 优化.\n- [ ] 清理垃圾代码.\n- [ ] 模板化.\n- [ ] 替换普通指针为智能指针.\n- [ ] 完善文档.\n- [x] 添加快速开始.\n- [ ] 增加更多测试案例.\n- [ ] 使用合适的设计模式.\n\n\n## 参考:\n\n1. 《惯性导航》秦永元\n2. 《捷联惯性导航技术(第2版 译本)》译者:张天光/王秀萍/王丽霞 作者:DavidH.Titte\n3. [An efficient orientation filter for inertial and\n inertial/magnetic sensor arrays](http://x-io.co.uk/res/doc/madgwick_internal_report.pdf)\n4. [Estimation of IMU and MARG orientation using a gradient descent algorithm](http://vigir.missouri.edu/~gdesouza/Research/Conference_CDs/RehabWeekZ%C3%BCrich/icorr/papers/Madgwick_Estimation%20of%20IMU%20and%20MARG%20orientation%20using%20a%20gradient%20descent%20algorithm_ICORR2011.pdf)\n5. [Direction Cosine Matrix IMU Theory](https://www.researchgate.net/publication/265755808_Direction_Cosine_Matrix_IMU_Theory)\n6. [METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS](http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf)\n7. [A Calibration Algorithm for Microelectromechanical Systems Accelerometers in Inertial Navigation Sensors](https://arxiv.org/pdf/1309.5075.pdf)\n8. [A Calibration Method of Three-axis Magnetic Sensor Based on Ellipsoid Fitting](https://www.researchgate.net/publication/273845104_A_Calibration_Method_of_Three-axis_Magnetic_Sensor_Based_on_Ellipsoid_Fitting)\n9. [Accuracy Improvement of Low Cost INS/GPS for Land Applications](https://prism.ucalgary.ca/bitstream/handle/1880/41142/2001_Shin.pdf?sequence=1)\n10. [Trajectory preprocessing: Computing with Spatial Trajectories](https://books.google.com.hk/books?hl=zh-CN&lr=&id=JShQJF23xBgC&oi=fnd&pg=PR3&dq=Trajectory+preprocessing.+Computing+with+Spatial+Trajectories&ots=6NUeew5i9_&sig=o7XM_QcuUnmOv5KNeezTN4H8PMw&redir_esc=y&hl=zh-CN&sourceid=cndr#v=onepage&q=Trajectory%20preprocessing.%20Computing%20with%20Spatial%20Trajectories&f=false)"
},
{
"path": "config/CMakeLists.txt",
"content": "aux_source_directory(. config_src_lists)\nadd_library(Location_config ${config_src_lists})"
},
{
"path": "config/Config.cpp",
"content": "//\n// Created by yangcheng on 2019/3/22.\n//\n\n#include \n#include \"Config.h\"\n#include \n#include \"iostream\"\n#include \n\nusing namespace std;\n\nclass FileNotFoundException : public exception {\n virtual const char* what() const throw()\n {\n return \"file not found!\";\n }\n};\n\nConfig::Config(string file_name, string delimiter, string comment) : delimiter(delimiter), comment(comment) {\n std::ifstream in_file(file_name);\n cout << file_name << endl;\n if (!in_file) throw FileNotFoundException();\n in_file >> (*this);\n in_file.close();\n}\n\n\nistream &operator>>(istream &is, Config &cfg) {\n\n string line;\n while (getline(is, line)) {\n\n // 忽略开头注释和空行\n string::size_type pos = line.find(cfg.comment);\n if (line.length() == 0 || pos == 0) continue;\n\n // 忽略末尾注释\n string param = line;\n if (pos < string::npos) param = line.substr(0, pos + cfg.comment.length());\n\n // key value截取\n string::size_type pos2 = param.find(cfg.delimiter);\n if (pos2 < string::npos) {\n string key = param.substr(0, pos2);\n string value = param.replace(0, pos2 + 1, \"\");\n // 删除前后空白和一些特殊字符\n cfg.Trim(key);\n cfg.Trim(value);\n cfg.cfg_settings[key] = value;\n }\n }\n return is;\n}\n\nostream &operator<<(std::ostream &os, const Config &cfg) {\n map::const_iterator key_value;\n for (key_value = cfg.cfg_settings.begin(); key_value != cfg.cfg_settings.end(); ++key_value) {\n os << key_value->first << \" : \" << key_value->second << endl;\n }\n return os;\n}\n\nvoid Config::Trim(std::string &s) {\n // 删掉头尾的特殊符号\n const char whitespace[] = \" \\n\\t\\v\\r\\f\";\n s.erase(0, s.find_first_not_of(whitespace));\n s.erase(s.find_last_not_of(whitespace) + 1);\n}\n\nvoid Config::SetDelimiter(std::string &d) {\n this->delimiter = d;\n}\n\nvoid Config::SetComment(std::string &c) {\n this->comment = c;\n}\n\nstd::string Config::Read(const std::string &key) const {\n auto value = this->cfg_settings.find(key);\n if (value != this->cfg_settings.end()) {\n return value->second;\n } else {\n return \"\";\n }\n}\n\nbool Config::FileExist(const std::string file_name) {\n bool exist = false;\n ifstream in(file_name);\n if (in) {\n exist = true;\n }\n in.close();\n return exist;\n}\n\nvoid Config::ReadFile(const std::string file_name, const std::string delimiter, const std::string comment) {\n this->delimiter = delimiter;\n this->comment = comment;\n ifstream in(file_name);\n if (!in) {\n throw \"file not found\";\n } else {\n in >> (*this);\n }\n}\n\n"
},
{
"path": "config/Config.h",
"content": "//\n// Created by yangcheng on 2019/3/22.\n//\n\n#ifndef LOCATION_CONFIG_H\n#define LOCATION_CONFIG_H\n\n\n#include \n#include