多传感器融合记录

多传感器信息融合的典型应用_多传感器融合中的时间硬同步1-论文阅读_weixin_39606911的博客-CSDN博客前言阅读硕士论文《自动驾驶中多传感器集成同步控制器设计与实现》，该论文为自动驾驶设计了一套时间同步控制器，涉及到的细节非常丰富，可以为多传感器融合中的时间同步提供设计思路。主要内容为实现多传感器数据的有效融合，首先需要实现多传感器的高度集成，同时采用一定的技术手段实现各传感的多源数据统一于同一时间和空间基准，保证各传感器数据同步，从而才能更有效的、准确的实现对自动驾驶车辆的精确定姿和定位。其中空间...https://blog.csdn.net/weixin_39606911/article/details/111106659 gps/imu融合（卡尔曼滤波）学习笔记_归去来兮哈的博客-CSDN博客_gps imu融合定位算法航位推算因为imu更新频率很快（100Hz)，假设车辆为匀速匀角速度模型，分别为v和w，模型如下：GPS坐标变换GPS原始输出定位是基于WGS-84坐标的经纬度信息。GPS坐标系需要投影为平面地图才可以做航位推算。常用的投影方法有高斯克吕格投影，UTM投影，墨卡托投影等。墨卡托投影转换方式如下：其中 x、 y 是地面坐标系两轴坐标， lon 是经度， lat 是纬度， EARTH_RAD 是地球半径，取 EARTH_RAD 为 6378137 米。GPS/IMU耦合方式分为松耦合和紧耦合方https://blog.csdn.net/weixin_41690997/article/details/114001968

GPS的NMEA码的详细解释定义_jickjiang的博客-CSDN博客_nmea版权声明:以下文字及图片属于网络摘录,飞扬科技编辑及理理,版权所有仍由原创作人持有-------------------------------------------------------------------------------------------------------NEMA协议的由来NMEA协议是为了在不同的GPS（全球定位系统）导航设备中建立统一https://blog.csdn.net/jickjiang/article/details/79086202

NMEA-0183信息整理与分析_Stupid_Gao的博客-CSDN博客主要针对NMEA协议进行了整理。分为三个基础数据梳理函数和五个常用GPS解析函数。 三个基础数据处理函数： 1：逗号位置处理函数：//从buf里面得到第cx个逗号所在的位置//返回值:0~0XFE,代表逗号所在位置的偏移.// 0XFF,代表不存在第cx个逗号 u8 NMEA_Comma_Pos(u8 *buf,u8 cx)//buf为指针变量，cx为第几个逗号 2.m^nhttps://blog.csdn.net/qq_34071268/article/details/78604839?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-4.no_search_link&spm=1001.2101.3001.4242.3
自动驾驶系统入门（七）- 多传感器信息融合（MSIF） - 知乎1、MISF - Multi-sensor Information Fusion 基础解析 1.1 基本概念利用计算机技术将多传感器或多源的信息和数据，在一定的准则下加以自动分析和综合，以完成所需的决策和估计而进行的信息处理过程；多传感器信息…https://zhuanlan.zhihu.com/p/96881390

多传感器融合详解 - 古月居多传感器融合详解https://www.guyuehome.com/34891

引言

1.2 融合层次（级别）

在多传感器信息融合中，按其在融合系统中信息处理的抽象程度可分为三个层次：数据级融合、特征级融合和决策级融合；

1）数据级融合：

—— 也称为像素级融合，属于底层数据融合；将多个传感器的原始观测数据(raw data)直接进行融合，然后再从融合数据中提取特征向量进行判断识别；

—— 数据级融合要求多个传感器是同质的（传感器观测的是同一物理量），否则需要进行尺度校准。

—— 数据级融合不存在数据丢失的问题，得到的结果也作为准确；但是计算量大，对系统通信带宽要求较高；

多传感器数据级融合基本原理示意图

2）特征级融合

属于中间层次级融合，先从每个传感器提供的原始观测数据中提取代表性的特征，再把这些特征融合成单一的特征向量；其中选择合适的特征进行融合是关键；特征信息包括边缘、方向、速度、形状等。

特征层融合可划分为两大类：目标状态融合、目标特性融合。

目标状态融合：主要应用于多传感器的目标跟踪领域；融合系统首先对传感器数据进行预处理以完成数据配准，在数据配准之后，融合处理主要实现参数关联和状态估计。

目标特性融合：就是特征层联合识别，它的实质就是模式识别问题；在融合前必须先对特征进行关联处理，再对特征矢量分类成有意义的组合；

在融合的三个层次中，特征层融合技术发展较为完善，并且由于在特征层已建立了一整套的行之有效的特征关联技术，可以保证融合信息的一致性；此级别融合对计算量和通信带宽要求相对降低，但由于部分数据的舍弃使其准确性也有所下降。

多传感器特征级融合基本原理示意图

3）决策级融合

—— 属于高层次级融合，是对数据高层次级的抽象，输出是一个联合决策结果，在理论上这个联合决策应比任何单传感器决策更精确或更明确；

—— 决策层融合在信息处理方面具有很高的灵活性，系统对信息传输带宽要求很低，能有效地融合反映环境或目标各个侧面的不同类型信息，而且可以处理非同步信息；

—— 由于环境和目标的时变动态特性、先验知识获取的困难、知识库的巨量特性、面向对象的系统设计要求等，决策层融合理论与技术的发展仍受到一定的限制；

多传感器决策级融合基本原理示意图

1.3 体系结构

根据对原始数据处理方法的不同，多传感器信息融合系统的体系结构可分为三种：集中式、分布式和混合式。

1）集中式 — 将各传感器获得的原始数据直接送到中央处理器进行融合处理，可以实现实时融合；

集中式体系结构示意图

优点：

a、结构简单，其数据处理的精度高，算法灵活，融合速度快；

缺点：

a、各传感器的流向是由低层向融合中心单向流动，各传感器之间缺乏必要的联系；

b、中央处理器计算和通信负担过重，系统容错性差，系统可靠性较低；

2）分布式 — 先对各个独立传感器所获得的原始数据进行局部处理，然后再将结果送入中央处理器进行融合处理来获得最终的结果；

分布式体系结构示意图

优点：

a、每个传感器都具有估计全局信息的能力，任何一种传感器失效都不会导致系统崩溃，系统可靠性和容错性高；

b、对通信带宽要求低，计算速度快，可靠性和延续性好；

缺点：

a、传感器模块需要具备应用处理器，这样的话自身的体积将更大，功耗也就更高；

b、中央处理器只能获取各个传感器经过处理后的对象数据，而无法访问原始数据；因此，想要“放大”感兴趣的区域将很难实现；

3）混合式 — 集中式和分布式的混合应用，即部分传感器采用集中式融合方式，剩余的传感器采用分布式融合方式；

混合式体系结构示意图

特点：

a、兼顾了集中式融合和分布式的优点，稳定性强，且具有较强的使用能力；

b、对通信带宽和计算能力要求较高；

2、自动驾驶感知模块多传感器数据融合

2.1 融合的先决条件

1）运动补偿

a、ego motion 自身运动补偿

即考虑传感器在采集过程中的某一时间戳内，由于车辆自身的运动，采集的对象会在该时间戳内发生相对位移变化；

例如：以激光雷达为例，采集一圈需要0.1s，在这0.1s内，车身本身会发生一定的位移，如果不考虑车辆本身位移的情况，检测出来的目标位置就会产生较大误差；

b、motion from others 来自于其他目标的运动补偿

即考虑传感器在采集过程中的某一时间戳内，运动物体由于自身运动会产生相对位移变化；

2）时间同步

通过统一的主机给各个传感器提供基准时间，各传感器根据已经校准后的各自时间为各自独立采集的数据加上时间戳信息，以做到所有传感器时间戳同步；但由于各个传感器各自采集周期相互独立，无法保证同一时刻采集相同的信息。

以GPS时间戳为基准的时间同步方法示意图

在很多自动驾驶车辆的传感器中，大部分支持GPS时间戳的时间同步方法；

GPS 的PPS信号与GPRMC融合，项目中底层板子为M7带有RTK、IMU、ODOM，将其根据PPS与GPRMC获取的时间，进行时间标定，将时间信息与传感器信息通过USB口传到导航板，同时将PPS接入到TX2 NX板。

linux ros USB阻塞与非阻塞记录_JanKin_BY的博客-CSDN博客

见程序中PPS部分。

3）空间同步 - 将不同传感器坐标系的测量值转换到同一个坐标系中；

—— 传感器标定是自动驾驶的基本需求，良好的标定是多传感器融合的基础, 自动驾驶车辆上的多个/多种传感器之间的坐标关系是需要确定的。

—— 外参是决定传感器和外部某个坐标系的转换关系，比如姿态参数。

例如：摄像机和雷达的融合， 需要建立精确的雷达坐标系、三维世界坐标系、摄像机坐标系、图像坐标系和像素坐标系之间的坐标转换关系；

激光雷达和相机联合标定示意图

2.2 多传感器融合基本思路示例

1）毫米波雷达和摄像头数据融合基本思路

—— 这两种传感器相融合，多数情况下都是以摄像头数据为主，毫米波雷达作为辅助；

—— 将毫米波雷达返回的目标点投影到图像上，围绕该点并结合先验知识，生成一个矩形的感兴趣区域，然后我们只对该区域内进行目标检测。

—— 他们融合的优点是可以迅速地排除大量不会有目标的区域，极大地提高识别速度。

2）摄像头和激光雷达数据融合基本思路

—— 障碍物的检测可以使用激光雷达进行物体聚类，但是对于较远物体过于稀疏的激光线数聚类的效果较差，因此利用视觉图像信息进行目标检测，进而获取障碍物的位置，同时视觉还可以给出障碍物类别信息；

—— 融合的关键是需要将摄像头和激光雷达进行联合标定，获取两者坐标系的空间转换关系；可以通过标定的方式，把激光雷达投射到图像的坐标系中，建立图像的像素点，和激光雷达投影后的点之间做匹配，然后通过某种优化方程，来解决匹配问题；

—— 激光雷达可以得到目标的3D 数据（x, y , z)，通过标定参数，以及相机本身的内参，可以把激光雷达的3D点投射到图像上，图像上的某些像素就会获得激光雷达的深度信息，然后便可以做基于图像的分割或者深度学习模型；

—— 融合过程中的时候，因为两者视场角的不同，可能会造成噪点或者漏点；

相机和激光雷达融合效果示意图

2）imu标定
    https://blog.csdn.net/OptimusAlpha/article/details/122325372

3）激光雷达和毫米波雷达融合的基本思路

激光雷达和毫米波雷达融合效果示意图

4）相机、imu时间同步：kalibr标定

源程序下载：git clone https://github.com/ethz-asl/Kalibr.git

opencv版本：opencv 3.2.0单独编译

opencv安装(x86/tx2 cuda/共享库)_JanKin_BY的博客-CSDN博客https://blog.csdn.net/JanKin_BY/article/details/120143123?spm=1001.2014.3001.5501修改：

      Kalibr/opencv2_catkin/cmake/opencv2-extras.cmake.in

     Kalibr/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt

添加：

     set(OpenCV_DIR  /sd_card/opencv-3.2.0/build)

sudo apt-get install python-catkin-tools

catkin build -DCMAKE_BUILD_TYPE=Release -j8
fatal error: numpy/arrayobject.h: No such file or directory
解决方法：
sudo apt-get install --reinstall python-numpy

问题：no Import pyx
解决：sudo apt-get install python-pyx
生成标定板：
usage:
    Example Aprilgrid:
        kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.08 --tspace 0.3
    Example Checkerboard:
        kalibr_create_target_pdf --type checkerboard --nx 8 --ny 8 --csx 0.03 --csy 0.03
联合标定：
kalibr_calibrate_imu_camera --target target.yaml   --cam camchain.yaml  --imu imu.yaml   --bag camera_imu.bag

 RuntimeError: [ImuConfig Reader]: Could not read configuration from /sd_card/kalibr_ws/imu.yaml
修改imu.yaml文件：

accelerometer_noise_density: 4.8641695361661035e-03   #Noise density (continuous-time)
accelerometer_random_walk:   2.3944306307068487e-04   #Bias random walk
 
#Gyroscopes
gyroscope_noise_density:     5.4583665041817392e-04   #Noise density (continuous-time)
gyroscope_random_walk:       6.1968037914410386e-06   #Bias random walk
 
rostopic:                    /camera/imu      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

 camchain.yaml

cam0:
  camera_model: pinhole
  distortion_coeffs: [-0.0578556396067142, 0.0661046653985977, -0.000246514391619712, -0.0211114417761564]
  distortion_model: equidistant
  intrinsics: [383.740661621094, 383.420074462891, 317.631652832031, 255.134979248047]
  resolution: [640, 480]
  rostopic: /camera/color/image_raw

 target.yaml

target_type: 'checkerboard' #gridtype
targetCols: 11               #number of internal chessboard corners
targetRows: 8               #number of internal chessboard corners
rowSpacingMeters: 0.03      #size of one chessboard square [m]
colSpacingMeters: 0.03      #size of one chessboard square [m]

[ERROR] [1642488673.898836]: Optimization failed!
kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera.py文件，搜索timeOffsetPadding我的大概在198行。我先是输出它的值看了一下是0.03，然后增大此变量的值即可，我增大到了0.3

一、GPS 数据获取

GPS数据遵循NMEA-0183协议，该数据标准是由NMEA（National Marine Electronics Association，美国国家海事电子协会）于1983年制定的。统一标准格式NMEA-0183输出采用ASCII 码，其串行通信的参数为：波特率＝9600bps，数据位＝8bit，开始位=1bit，停止位＝1bit，无奇偶校验。
数据传输以“语句”的方式进行，每个语句均以“$”开头，然后是两个字母的“识别符”和三个字母的“语句名”，接着就是以逗号分割的数据体，语句末尾为校验和，整条语句以回车换行符结束。
NMEA-0183的数据信息有十几种，这些信息的作用分别是：$GPGGA：输出GPS的定位信息；$GPGLL：输出大地坐标信息；$GPZDA：输出UTC时间信息；$GPGSV：输出可见的卫星信息；$GPGST：输出定位标准差信息；$GPGSA：输出卫星DOP值信息；$GPALM：输出卫星星历信息；$GPRMC：输出GPS推荐的最短数据信息等。

根据逗号位置解析数据：

https://blog.csdn.net/qq_34071268/article/details/78604839?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-4.no_search_link&spm=1001.2101.3001.4242.3
//从buf里面得到第cx个逗号所在的位置
//返回值:0~0XFE,代表逗号所在位置的偏移.0XFF,代表不存在第cx个逗号   
u8 NMEA_Comma_Pos(u8 *buf,u8 cx) //buf是指针变量
{
    u8 *p=buf;//指针变量地址给了值，p表示啥，p也是指针变量,其实这个是两条语句 u8
    while(cx)
    {
        if(*buf=='*'||*buf<''||*buf>'z') return OXFF;//遇到‘*‘或者非法字符，则不存在第cx个逗号
        if(*buf==',')cx--;
        buf++;//指针变量移动位置
    }
    return buf-p;   
}

//m^n函数  返回值:m^n次方.
u32 NMEA_Pow(u8 m,u8 n)
{
    u32 result = 1;
    while(n--)result*=n;
    return result   
}

//str转换为数字
//buf:数字存储区
//dx:小数点位数，返回给调用函数 //小数点位数
//返回值：转换后的数值
//应该是数值，不含小数，但是你知道了位数，位数在形参指针里
int NMEA_Str2num(u8 *buf,u8*dx);//buf 和 dx 都是指针变量。res是16位的
{
    u8 *p=buf;//p变为指针变量
    u32 ires=0,fres=0;
    u8 ilen=0,flen=0,i;
    u8 mask = 0;//mask = 0x00;
    int res;
    while(1)//得到整数和小数的长度 不停的进行遍历
    {
        if(*p=='-'){mask|=0x02;p++;}//是负数。mask=0X02是负数
        if(*p==','||(*p=='*')){}break;//遇到结束了
        if(*p=='.'){mask|=0x01;p++;}//遇到小数点了。
        else if(*p>'9'||(*p<'0'))   //有非法字符
        {
            ilen = 0;
            flen = 0;
            break;
        }
        if (mask&0X01)flen++;
        else ilen++; //
        p++;
    }
    if(mask&0x02)flen++;//去掉负号，为啥这就是减掉符号了
    for(i=0;i5)flen=5;   //最多取5位小数
    for(i=0;i

一、 输出语句说明：

1.$GPRMC语句(经纬度，年月日时分秒，与pps信号结合做时间同步)

$GPRMC，<1>，<2>，<3>，<4>，<5>，<6>，<7>，<8>，<9>，<10>，，<12>
$GPRMC语句意义——取值范围
<1> UTC时间：hhmmss.ss——000000.00~235959.99
<2> 状态，有效性 ——A表示有效；V表示无效
<3> 纬度格式：ddmm.mmmm——0000．00000~8959.9999 （标准的度分格式）
<4> 南北半球——N北纬；S南纬
<5> 经度格式：dddmm.mmmm——00000.0000~17959.9999 （标准的度分格式）
<6> 东西半球——E表示东经；W表示西经
<7> 地面速度——000.00~999.999
<8> 速度方向——000.00~359.99
<9> 日期格式，月日年——010100~123199
<10> 磁偏角，单位：度——00.00~99.99
<11>磁偏角方向——E表示东；W表示西
<12> 模式指示及校验和—— A=自主定位，D=差分，E=估算，N=数据无效

例如：$GPRMC,074529.82,A,2429.6717,N,11804.6973,E,12.623,32.122,010806,,W,A*08

 程序：

void NMEA_GNRMC_Analysis(nmea_msg *gpsx,u8 *buf)
{
    u8 *p1,dx;           
    u8 posx;     
    u32 temp; //32位变量去读取时间数据，格式是不清楚的，实际数字例如095554.000      
    float rs;  
    p1=(u8*)strstr((const char *)buf,"$GNRMC");//"$GNRMC",经常有&和GNRMC分开的情况,故只判断GPRMC.
    posx=NMEA_Comma_Pos(p1,1);                              //得到UTC时间
    if(posx!=0XFF)
    {   //这里数据存在一个算法，我不知道这个时间是什么原理，但是能算出来
        temp=NMEA_Str2num(p1+posx,&dx)/NMEA_Pow(10,dx);     //得到UTC时间,去掉ms
        gpsx->utc.hour=temp/10000;//95554/10000 = 9
        gpsx->utc.min=(temp/100)%100;//955%100 = 55
        gpsx->utc.sec=temp%100;  //54    
    }   
    posx=NMEA_Comma_Pos(p1,3);  //得到纬度 ddmm.mmmmm（度分）【2318.1327】
    if(posx!=0XFF)
    {
        temp=NMEA_Str2num(p1+posx,&dx);//取整2318          
        gpsx->latitude=temp/NMEA_Pow(10,dx+2);  //得到°//23°
        rs=temp%NMEA_Pow(10,dx+2);              //得到'//2318%（10^2）=18'  
        //【这句子历程是5，我跟人感觉好像是6呢，好奇怪，这里怎么算出来的】     
        gpsx->latitude=gpsx->latitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为° 
    }
    posx=NMEA_Comma_Pos(p1,4);                              //南纬还是北纬，确定位置 
    if(posx!=0XFF)gpsx->nshemi=*(p1+posx);                   
    posx=NMEA_Comma_Pos(p1,5);                              //得到经度
    if(posx!=0XFF)
    {                                                 
        temp=NMEA_Str2num(p1+posx,&dx);          
        gpsx->longitude=temp/NMEA_Pow(10,dx+2); //得到°
        rs=temp%NMEA_Pow(10,dx+2);              //得到'        
        gpsx->longitude=gpsx->longitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为° 
    }
    posx=NMEA_Comma_Pos(p1,6);                              //东经还是西经
    if(posx!=0XFF)gpsx->ewhemi=*(p1+posx);       
    posx=NMEA_Comma_Pos(p1,9);                              //得到UTC日期
    if(posx!=0XFF)
    {
        temp=NMEA_Str2num(p1+posx,&dx);                     //得到UTC日期
        gpsx->utc.date=temp/10000;
        gpsx->utc.month=(temp/100)%100;
        gpsx->utc.year=2000+temp%100;        
    } 
}

2.$GPGGA语句: GPS定位主要数据，该语句中包括经纬度、质量因子、HDOP、高程、基准站号等字段。下表详细说明GPGGA语句中的各个字段：

$GPGGA，<1>，<2>，<3>，<4>，<5>，<6>，<7>，<8>，<9>，<10>，，<12>,，<14>
$GPGGA语句意义——取值范围
<1> UTC时间：hhmmss.ss——000000.00~235959.99
<2> 纬度，格式：ddmm.mmmm ——0000．00000~8959.9999 （标准的度分格式）
<3> 南北半球——N北纬；S南纬
<4> 经度格式：dddmm.mmmm ——00000.0000~17959.9999 （标准的度分格式）
<5> 东西半球——E表示东经；W表示西经
<6> 质量因子——0=未定位，1=GPS单点定位固定解，2=差分定位，3=PPS解；4=RTK固定解；5=RTK浮点解；6=估计值；7=手工输入模式；8=模拟模式；
<7> 应用解算位置的卫星数——00~12
<8> HDOP，水平图形强度因子——0.500~99.000 ；大于6不可用
<9> 天线高程（海平面）——－9999.9～99999.9
<10> 线线高程单位(m) ——m
<11> 大地椭球面相对海平面的高度（-9999.9到9999.9）。
<12> 大地水准面起伏单位(m)   ——m
<13> 差分GPS数据期——差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空），不使用DGPS时为空
<14> 基准站号——0000~1023；不使用DGPS时为空

 $GPGGA,074529.82,2429.6717,N,11804.6973,E,1,8,1.098,42.110,M,,M,,*76

程序：

void NMEA_GNGGA_Analysis(nmea_msg *gpsx,u8 *buf)
{
    u8 *p1,dx;  //    
    u8 posx;    //是变量
    p1=(u8*)strstr((const char *)buf,"$GNGGA");
    //从首地址放入一般都是GPS所有数据，然后的到p1为$GNGGA的首地址
    posx=NMEA_Comma_Pos(p1,6);//得到GPS状态//代表逗号所在位置的偏移
    if(posx!=0XFF)gpsx->gpssta=NMEA_Str2num(p1+posx,&dx);   buf:数字存储区,dx:小数点位数
    posx=NMEA_Comma_Pos(p1,7);                              //得到用于定位的卫星数
    if(posx!=0XFF)gpsx->posslnum=NMEA_Str2num(p1+posx,&dx); 
    posx=NMEA_Comma_Pos(p1,9);                              //得到海拔高度
    if(posx!=0XFF)gpsx->altitude=NMEA_Str2num(p1+posx,&dx);  
}

3.$GPGSV语句:星的方位角、俯仰角、信噪比等每条语句最多包括四颗卫星的信息，每颗卫星的信息有四个数据项，即：(4)－卫星号，(5)－仰角，(6)－方位角，(7)－信噪比

$GPGSV,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>,<13>,<14>,<15>,<16>,<17>,<18>,<19>
$GPGSV语句意义——取值范围
<1> 总的GSV语句电文数——0~12
<2> 当前GSV语句号
<3> 可视卫星总数——0~32
<4> 卫星号——1~32
<5> 仰角——00～90
<6> 方位角——000～359
<7> 信噪比——00～99dB无表未接收到讯号
<8> 卫星号——1~32
<9> 仰角——00～90
<10> 方位角——000～359
<11> 信噪比——00～99dB无表未接收到讯号
<12> 卫星号——1～32
<13> 仰角——00～90
<14> 方位角——000～359
<15> 信噪比——00～99dB 无表示未接收到讯号
<16> 卫星号——1～32
<17> 仰角——00～90
<18> 方位角——000～359
<19> 效验和，格式：*效验和——检查位

$GPGSV,3,1,11,1,83,54,32,3,19,192,28,6,26,57,36,7,51,140,37*7D
$GPGSV,3,2,11,14,40,136,34,16,64,266,36,20,21,293,,22,2,168,*4C
$GPGSV,3,3,11,23,10,321,,25,53,7,40,30,2,46,*48

程序：

void NMEA_GPGSV_Analysis(nmeg_msg *gpsx,u8 *buf)
{
    u8 *p,*p1,dx;
    u8 len,i,j,slx=0;
    u8 posx;
    p=buf;
    p1=(u8*)strstr((const char *)p,"$GPGSV");//p1应该是$的指针变量
    len=p1[7]-'0';//得到GPGSV的条数，应该是3,看例子
    posx=NMEA_Comma_Pos(p1,3); //得到可见卫星总数【11】
    if(posx!=0XFF)gpsx->svnum=NMEA_Str2num(p1+posx,&dx);//
    for(i=0;islmsg[slx].num=NMEA_Str2num(p1+posx,&dx);   //得到卫星编号
            else break; 
            posx=NMEA_Comma_Pos(p1,5+j*4);
            if(posx!=0XFF)gpsx->slmsg[slx].eledeg=NMEA_Str2num(p1+posx,&dx);//得到卫星仰角 
            else break;
            posx=NMEA_Comma_Pos(p1,6+j*4);
            if(posx!=0XFF)gpsx->slmsg[slx].azideg=NMEA_Str2num(p1+posx,&dx);//得到卫星方位角
            else break; 
            posx=NMEA_Comma_Pos(p1,7+j*4);
            if(posx!=0XFF)gpsx->slmsg[slx].sn=NMEA_Str2num(p1+posx,&dx);    //得到卫星信噪比
            else break;
            slx++;     
        }   
        p=p1+1;//切换到下一个GPGSV信息  
    }
}

4.$GPVTG语句(Track Made Good and Ground Speed－VTG，地面速度信息）

格式：$GPVTG,<1>,T,<2>,M,<3>,N,<4>,K,<5>*hh
字段 $GPVTG语句意义——取值范围
<1> 以真北为参考基准的地面航向——000.000~359.999
<2> 以磁北为参考基准的地面航向——000.000~359.999
<3> 地面速率——000.000~999.999节
<4> 地面速率——0000.0~1851.8公里/小时
<5> 模式指示——A=自主定位，D=差分，E=估算，N=数据无效(仅NMEA0183 3.00版本输出)
<6> hh 校检位

$GPVTG,257.314,T,257.314,M,10.739,N,19.888,K,A*2F

程序：

void NMEA_GNVTG_Analysis(nmea_msg *gpsx,u8 *buf)
{
    u8 *p1,dx;           
    u8 posx;    
    p1=(u8*)strstr((const char *)buf,"$GNVTG");    
    //$GNVTG,(1),T,(2),M,(3),N,(4),K,(5)*hh(CR)(LF)    
    //第7个逗号后面是（4）即地面速率
    posx=NMEA_Comma_Pos(p1,7);                              //得到地面速率
    if(posx!=0XFF)
    {
        gpsx->speed=NMEA_Str2num(p1+posx,&dx);              //得到速度，后面为啥扩大1000倍表示不理解
        if(dx<3)gpsx->speed*=NMEA_Pow(10,3-dx);             //确保扩大1000倍
    }
} 

5.$GPGSA语句（GPS DOP and Active Satellites－GSA，当前卫星信息）

GSA : GNSS 的当前卫星和精度因子,包括可见卫星PRN号,以及PDOP、HDOP、VDOP。如：
<1>     模式 ——M = 手动， A = 自动。
<2>     定位类型——1 = 未定位， 2 = 二维定位， 3 = 三维定位。
<3>     PRN 数字——01 至 32 表天空使用中的卫星编号，最多可接收12颗卫星信息。正在用于解算位置的卫星号（01~32，前面的0也将被传输）。
<4>     PDOP位置精度因子——0.5～99.9  
<5>     HDOP水平精度因子——0.5～99.9
<6>     VDOP垂直精度因子——0.5～99.9
<7>     Checksum.(检查位).

$GPGSA,<1>,<2>,<3>,<3>,,,,,<3>,<3>,<3>,<4>,<5>,<6>,<7>
$GPGSA,A,3,19,11,03,23,27,13,16,,,,,,3.43,1.67,2.99*0E

程序：

void NMEA_GNGSA_Analysis(nmea_msg *gpsx,u8 *buf)
{
    u8 *p1,dx;  //dx为后面用的但没有体现小数点右面保留几位啊     
    u8 posx; //
    u8 i;   
    p1=(u8*)strstr((const char *)buf,"$GNGSA");//先得到定位类型
    posx=NMEA_Comma_Pos(p1,2);  //得到定位类型
    if(posx!=0XFF)gpsx->fixmode=NMEA_Str2num(p1+posx,&dx); //确定定位类型 
    for(i=0;i<12;i++)                                       //得到定位卫星编号
    {
        posx=NMEA_Comma_Pos(p1,3+i);                     
        if(posx!=0XFF)gpsx->possl[i]=NMEA_Str2num(p1+posx,&dx); //将卫星编号逐1手机过来
        else break; 
    }                 
    posx=NMEA_Comma_Pos(p1,15);                             //得到PDOP位置精度因子
    if(posx!=0XFF)gpsx->pdop=NMEA_Str2num(p1+posx,&dx);  
    posx=NMEA_Comma_Pos(p1,16);                             //得到HDOP位置精度因子
    if(posx!=0XFF)gpsx->hdop=NMEA_Str2num(p1+posx,&dx);  
    posx=NMEA_Comma_Pos(p1,17);                             //得到VDOP位置精度因子
    if(posx!=0XFF)gpsx->vdop=NMEA_Str2num(p1+posx,&dx);  
}

6.$GPGLL语句（输出大地坐标信息）

$GPGLL,<1>,<2>,<3>,<4>,<5>,<6>,<7>*61
字段 $GPGLL语句意义——取值范围
<1> 纬度：ddmm.mmmmm——0000．00000~8959.9999
<2> 南纬或北纬——北纬N，S南纬
<3> 经度：dddmm.mmmmm——0000．00000~17959.99999
<4> 东、西经 ——东经E，西经W
<5> UTC时间——hh:mm:ss
<6> 数据状态——A有效，V无效

$GPGLL,2431.25310,N,11806.15429,E,081401.00,A,A*61

7.$GPZDA 语句（输出UTC时间和日期信息）

$GPZDA, <1>,<2>,<3>,<4>,<5>*hh
$GPZDA语句意义——取值范围
<1> 时间：hhmmss.ss——0000000.00～235959.99
<2> 日——00～31
<3> 月——00～12
<4> 年——0000～9999
<5> 地方时与世界时之差
<6> 检校位 *hh——*

$GPZDA,081401.00,14,09,2006,00,00*62

8.$GPGST 语句（GPS 伪距噪声统计,包括了三维坐标的标准偏差信息）

字段——示例——说明
Sentence ID ——$GPGST
UTC Time——024603.00——UTC time of associated GGA fix
RMS deviation——3.2——Total RMS standard deviation of ranges inputs to the navigation solution
Semi-major deviation——6.6——Standard deviation (meters) of semi-major axis of error ellipse
Semi-minor deviation——4.7——Standard deviation (meters) of semi-minor axis of error ellipse
Semi-major orientation——47.3——Orientation of semi-major axis of error ellipse (true north degrees)
Latitude error deviation——5.8——Standard deviation (meters) of latitude error
Longitude error deviation——5.6——Standard deviation (meters) of longitude error
Altitude error deviation——22.0——Standard deviation (meters) of latitude error
Checksum—— *58

$GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58

9.$GPCN0语句(各颗用于解算的卫星信噪比)

$GPCN0,30,40,35,26,24,36,31,38

二、 输出时间说明：

与产品相关：

GPS帧率：5hz

IMU:100HZ

CAMERA:30HZ

ODOM:100HZ

如和芯星通下载中心_服务_和芯星通_北斗导航芯片-北斗导航模块北斗高精度定位板卡国内外领先的芯片、OEM板卡和产品解决方案提供商

 UB4B0/UB482

三、坐标系统WGS84

WGS－84坐标系（World Geodetic System）是一种国际上采用的地心坐标系。坐标原点为地球质心，其地心空间直角坐标系的Z轴指向国际时间局（BIH）1984.0定义的协议地极（CTP）方向，X轴指向BIH1984.0的协议子午面和CTP赤道的交点，Y轴与Z轴、X轴垂直构成右手坐标系，称为1984年世界大地坐标系。这是一个国际协议地球参考系统（ITRS），是目前国际上统一采用的大地坐标系。GPS广播星历是以WGS-84坐标系为根据的。
WGS84坐标系，长轴6378137.000m，短轴6356752.314，扁率1/298.257223563。

DOP值有三种测量类型：
水平、垂直和说明（mean）。HDOP测量它和经纬度的关系作为精度，VDOP测量他和海拔的关系作为精度，PDOP，给出一个关于经纬度、海拔精度的完整等级。每个DOP值都是1到50之间数值，其中50 表示非常差的精度，1则表示很理想的精确度。
表2-1列出了划分的一份精确的PDOP值细目分类。PDOP分类描述：
PDOP值——评价——说明
1～2——理想——这是最高的信任级别，应用程序可以在任何时候获取最高的精度
2～3——极好——值得信任的级别，位置的测量相当精确，可以应用于绝大多数应用程序，但是有些非常敏感的应用程序就不行了
4～6——好——这是用来做商业应用的最低等级。位置的测量可以用来做线路导航建议
7～8——中等——位置测量可以用来计算，但是质量还应该提高。推荐用语天空观察
9～20——中下——信任级别很低。位置测量结果应该丢弃或者仅用于粗略估计当前位置
21～50——差——在这个级别的测量结果是不精确的，大概会有半个足球场大的误差，应该丢弃

ecef gps2ecef(gpsMessage gpsMsg){       //  gpsMsg是上面那个经纬高数据
    double a = 6378137;
    double b = 6356752.3142;
    double f = (a - b) / a;
    double e_sq = f * (2 - f);
    double lamb = gpsMsg.lat/180*pi;
    double phi = gpsMsg.lon/180*pi;
    double s = sin(lamb);
    double N = a / sqrt(1 - e_sq * s * s);
    ecef Xy;
    double sin_lambda = sin(lamb);
    double cos_lambda = cos(lamb);
    double sin_phi = sin(phi);
    double cos_phi = cos(phi);

    Xy.x = (gpsMsg.height + N) * cos_lambda * cos_phi;
    Xy.y = (gpsMsg.height + N) * cos_lambda * sin_phi;
    Xy.z = (gpsMsg.height + (1 - e_sq) * N) * sin_lambda;
    return Xy;
}

四.常用精度

PDOP：位置精度因子（Position Dilution of Precision），直译为“精度强弱度”，通常翻译为“相对误差”。具体含义是：由于观测成果的好坏与被测量的人造卫星和接收仪间的几何形状有关且影响甚大，所以计算上述所引起的误差量称为精度的强弱度。天空中卫星分布程度越好，定位精度越高（数值越小精度越高）。PDOP表示三维位置定位精度与导航台几何配置关系的一个参数。在全球定位(GPS)系统中，等于用户位置的径向误差(1°)与用户到卫星的距离测量误差(1°)的比值。
Pdop取值范围为：0.5--99.9，为纬度、经度和高程等误差平方和的开根号值，所以Pdop的平方 =Hdop 的平方 +Vdop 的平方。HDOP2+VDOP2=PDOP2
  PDOP( position dilution of precision ) 三维位置精度因子：为纬度、经度和高程等误差平方和的开根号值
 TDOP（time dilution of precision ）钟差精度因子：为接收仪内时表偏移误差值。

 HDOP（horizontal dilution of precision ）水平分量精度因子：为纬度和经度等误差平方和的开根号值。
    VDOP（vertical dilution of precision ）垂直分量精度因子

age:差分延时
VRMS:水平残差
HRMS:垂直残差
RMS ：HRMS2+VRMS2=RMS2

五.差分基站数据获取
 

分为两种：网络基站 自建基站

网络基站：如千寻

①地址 域名：rtk.ntrip.qxwz.com   或IP（60.205.8.49）
②端口
端口：8001   对应ITRF2008坐标系
端口：8002   对应WGS84
端口：8003   对应CGCS2000
③mountpoint/源/挂载点
RTCM32_GGB 或 RTCM30_GG
④差分账号
⑤差分账号密码

ROS与传感器教程-Ublox RTK GPS结合千寻服务基站使用 - 创客智造

自检基站：将另一块板卡设置为基站模式，通过远程透传到 移动站进行RTK结果输出。

ROS与传感器教程-两个Ublox RTK GPS分别做移动站和基站使用 - 创客智造

 六.GPS与IMU融合

ROS入门：IMU&GPS融合定位实例 - 古月居https://www.guyuehome.com/34589【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter)，扩展卡尔曼滤波，实现GPS+IMU融合，EKF ESKF GPS+IMU_一点儿也不萌的萌萌-CSDN博客_误差状态卡尔曼滤波https://blog.csdn.net/u011341856/article/details/114262451https://blog.csdn.net/u011341856/article/details/114262451https://blog.csdn.net/u011341856/article/details/114262451https://blog.csdn.net/u011341856/article/details/114262451 Error-State Kalman Filter教程(一)_浮生-CSDN博客_error state

机器人运动估计——IMU运动方程与ESKF原理介绍（下）_西涯先生的博客-CSDN博客_eskf算法

融合整体过程：

6.1预测

https://github.com/ydsf16/imu_gps_localization/blob/master/imu_gps_localizer/src/imu_processor.cpp

 17行

void ImuProcessor::Predict(const ImuDataPtr last_imu, const ImuDataPtr cur_imu, State* state)

6.2更新

https://github.com/ydsf16/imu_gps_localization/blob/master/ros_wrapper/src/localization_wrapper.cpp

 86行

imu_gps_localizer_ptr_->ProcessGpsPositionData(gps_data_ptr);

https://github.com/ydsf16/imu_gps_localization/blob/master/imu_gps_localizer/src/imu_gps_localizer.cpp

 51行

gps_processor_->UpdateStateByGpsPosition(init_lla_, gps_data_ptr, &state_);

imu_gps_localization/gps_processor.cpp at master · ydsf16/imu_gps_localization · GitHub

 9行

bool GpsProcessor::UpdateStateByGpsPosition(const Eigen::Vector3d& init_lla, const GpsPositionDataPtr gps_data_ptr, State* state) {
    Eigen::Matrix H;
    Eigen::Vector3d residual;
    ComputeJacobianAndResidual(init_lla, gps_data_ptr, *state, &H, &residual);
    const Eigen::Matrix3d& V = gps_data_ptr->cov;

    // EKF.
    const Eigen::MatrixXd& P = state->cov;
    const Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + V).inverse();
    const Eigen::VectorXd delta_x = K * residual;

    // Add delta_x to state.
    AddDeltaToState(delta_x, state);

    // Covarance.
    const Eigen::MatrixXd I_KH = Eigen::Matrix::Identity() - K * H;
    state->cov = I_KH * P * I_KH.transpose() + K * V * K.transpose();
}

绿线：gnss

红线：gnss与imu融合