949104476 / fused-localization

一种基于中心极限定理与直方图置信区间估计和均值中值滤波器的 Lidar/GNSS/INS 三定位数据的融合策略

Fused-Localization

一种基于中心极限定理与直方图置信区间估计和均值中值滤波器的 Lidar/GNSS/INS 三定位数据的融合策略

判断有效的思路： 1. GNSS失效的判据： sol_type的值 std_dev是否超过设定阈值 2. Lidar失效 残差是否超过设定阈值 Lidar与有效的INS输出四元数的差是否超过设定阈值 根据这个差值计算Lidar置信度，1最好0最差。 * 以上所说"设定阈值"均为对2000个样本点数学分析之后，得到的静态常数

相互校验的思路 1. GNSS状态好的时候，说明INS比较可信，可以用INS检验修正Lidar：通过计算的置信度conf调节计算 xyz时Lidar的浮动权重。 GNSS状态差的时候，说明INS不可信了，就要用Lidar检验修正INS：通过计算的置信度conf调节计算四元数时INS的浮动权重。

计算思路： 实时计算权重Ws = 预设权重W 与 浮动权重w 的差或积（要保证总权重不能大于预设权重） 浮动权重由自身的标准差std_dev/残差r和相互校验得到的置信度conf决定 实时输出融合值 = sum(Ws[i]* 值[i]) /sum(Ws);//加权平均

step1：计算Lidar数据的置信度conf
		conf = (Lidar与ins测量差值 - 设定阈值)/设定阈值
			*除以阈值的目的是归一化处理，1最好0最差。
			*需要分别计算QXQYQZQW四个置信度取平均值后返回一个conf
	

step2：计算(qx,qy,qz,qw)姿态四元数
	预设权重	  		  Lidar	INS
		GNSS=NARROW_INT  :	5	 5			//精度较好
		GNSS=NARROW_FLOAT:	+1	-1			//精度较差
		GNSS=PSRDIFF	 :  +2	-1			//精度很差	
		Lidar残差超阈值	 :  -2  +1 			//若Lidar不超过阈值，则为上面的三个
	Lidar的浮动权重w = 残差 * para 			//para为待定参数，我姑且先设定3
	GNSS可信的时候：INS没有浮动权重项，预设权重就是总权重
	GNSS不可信的时候：INS总权重 = INS预设权重 * conf，
	实时计算权重Ws = w + W
	计算输出四元数融合值：
		Q =  (Lidar的实时总权重Ws*Lidar测量值 + INS权重*INS测量值 )/(Lidar的实时总权重Ws + INS权重);
		

step3:计算(X,Y,Z)定位坐标
	预设权重	W  	  	  Lidar	GNSS INS
		GNSS=NARROW_INT  :	5	 5	  5		//GNSS精度较好
		GNSS=NARROW_FLOAT: +2	-1	 -1		//GNSS精度较差
		GNSS=PSRDIFF	 : +3	-2	 -1		//GNSS精度很差
		Lidar残差超阈值	 : -2   +1	 +1		//若Lidar不超过阈值，则为上面的三个
	GNSS浮动权重w = std_dev * para 			//para为待定参数，我姑且先设定4
	GNSS可信的时候：INS没有浮动权重项，预设权重就是总权重
					Lidar浮动权重w= conf * Lidar固定权重
	GNSS不信的时候：INS总权重 = INS预设权重 * conf，
					Lidar浮动权重w = para* 残差 
	计算输出坐标融合值：
		X = (Lidar的x实时总权重Ws*Lidar测量x + INS权重*INS测量x + GNSS的x的实时总权重Ws*GNSS测量的X)
			/(Lidar的x实时总权重Ws + INS权重 + GNSS的x的实时总权重Ws)

反思分析： 1. 需要调节的东西 · 固定权重的设置大小 · 计算某些项浮动权重的para的大小（就那些姑且先设定4啊3啊5啊的那些） · 判别阈值的设定：根据datasheet.histogram的直方图可以动态调一调 · 2. 自我校验 相互校验 INS: 无 Lidar GNSS: 标准差阈值 无 lidar： 残差阈值 INS 3.对于超出阈值的处理： 也未尝不可以取一点有用的东西出来呢？

一个完整过程表述的例子："1574149171.1" 时刻各传感器的数据如下 GNSS_POSE: GX GY GZ sol_type lat_std_dev lon_std_dev hi_std_dev 684705.352052 3112421.905949 41.150541 NARROW_INT 0.00585 0.00659 0.01560 INS_POSE IX IY IZ Iqx Iqy Iqz Iqw 684703.7752890 3112420.482600 41.835366 0.003103 0.007091 0.984194 0.176925
LidarPose LX LY LZ Lqx Lqy Lqz Lqw rx ry yz rw 684703.834915 3112420.472351 41.826940 0.004169 0.008907 0.984296 0.176257 0.067209 0.051231 0.038484 0.079860

(qx,qy,qz,qw)
	step1：计算Lidar数据的置信度conf  						//规定 Δqx阈值:0.032;Δqy阈值:0.0125;Δqz阈值:1.98;Δqw阈值:1.8
		confQX = fabs(fabs(0.004169-0.003103)-0.032)/0.032  = 0.966687;
		confQY = fabs(fabs(0.008907-0.007091)-0.0125)/0.0125 = 0.854720;
		confQZ = fabs(fabs(0.984296-0.984194)-1.98)/1.98 = 0.999948
		confQW = fabs(fabs(0.176257-0.176925)-1.8)/1.8=0.999629;
			conf = (0.966687+0.854720+0.999948+0.999629)/4=0.9552460	

	step2:计算权重
		预设权重	  	  	  Lidar INS
			GNSS=NARROW_INT  :	5	 5			//精度较好
		计算Lidar各浮动权重
			Lidar_qx的浮动权重wx = 0.067209 * 4	= 0.268836	//para姑且先设定4，此时rx=0.067209
			Lidar_qy的浮动权重wy = 0.051231 * 4 = 0.204924	//para姑且先设定4，此时rx=0.051231
			Lidar_qz的浮动权重wz = 0.038484 * 4	= 0.153936	//para姑且先设定4，此时rx=0.038484
			Lidar_qw的浮动权重ww = 0.079860 * 4	= 0.319440	//para姑且先设定4，此时rx=0.079860	
		GNSS和INS没有浮动权重项，故总权重就是预设权重
		计算Lidar总权重：
			Lidar_qx的总权重WSx = 5-0.268836 = 4.731164
			Lidar_qy的总权重WSy = 5-0.204924 = 4.795076	
			Lidar_qz的总权重WSz = 5-0.153936 = 4.846064
			Lidar_qw的总权重WSw = 5-0.319440 = 4.680560
		计算输出四元数融合值：///两个传感器的加权平均
			Qx =  (4.731164*0.004169 + 5*0.003103 )/(4.731164 + 5) = 0.003621;
			Qy =  (4.795076*0.008907 + 5*0.007091 )/(4.795076 + 5) = 0.007980;
			Qz =  (4.846064*0.984296 + 5*0.984194 )/(4.846064 + 5) = 0.984244;
			Qw =  (4.680560*0.176257 + 5*0.176925 )/(4.680560 + 5) = 0.176602;		
			
	step3:计算(X,Y,Z)定位坐标
		预设权重	W  	  	  Lidar	GNSS INS
			GNSS=NARROW_INT  :	5	 5	  5					//精度较好
		计算GNSS浮动权重
			GNSS_X 浮动权重wx = 0.00585 * 3 = 0.1755		//para为3，此时x的标准差lat_std_dev为0.00585
			GNSS_Y 浮动权重wy = 0.00659 * 3 = 0.01977		//para为3，此时y的标准差lon_std_dev为0.00659
			GNSS_Z 浮动权重wz = 0.01560 * 3 = 0.0468		//para为3，此时z的标准差hi_std_dev为0.01560		
		计算GNSS的实时总权重
			GNSS_X的总权重WSx = 5 - 0.1755 = 4.8245;
			GNSS_Y的总权重WSx = 5 - 0.01977 = 4.8023;
			GNSS_Y的总权重WSx = 5 - 0.0468 = 4.9532;				
		INS没有浮动权重项
		计算Lidar浮动权重（只有一个）
			Lidar浮动权重w = 0.9552460	*5 =4.77623
		计算输出坐标融合值：///三个传感器的加权平均
			X = (4.77623*684703.834915 + 5*684703.7752890 + 4.8245*684705.352052)/(4.77623 + 5 + 4.8245) = 684704.315801740 ;
			Y = (4.77623*3112420.472351+5*3112420.482600 + 4.8023*3112421.905949)/(4.77623 + 5 + 4.8023) = 3112420.948106280;
			Z = (4.77623*41.826940 +5*41.835366 + 4.9532*41.150541)/(4.77623 + 5 + 4.9532) 				 = 41.60234138845830;


融合输出数据和三个数据的比较
	GNSS_POSE:						
	GX				GY				GZ	 		
	684705.352052 	3112421.905949 	41.150541	
	INS_POSE						
	IX				IY				IZ	    			Iqx			Iqy    	   	Iqz   		Iqw
	684703.7752890	3112420.482600	41.835366			0.003103	0.007091	0.984194	0.176925  
	LidarPose										
	LX				LY				LZ					Lqx			Lqy			Lqz			Lqw			
	684703.834915 	3112420.472351 	41.826940 			0.004169	0.008907	0.984296	0.176257
	
	FusePose
	FX				FY				FZ					FQX			FQY			FQZ			FQW
	684704.31580174 3112420.9481063 41.6023413884583	0.003621	0.007980	0.984244	0.176602
	
结论：看起来还不错的亚子