从卫星接收机到组合导航【八】（零速判断）

零速运用场景

本系统为中低精度的光纤系统，想要获得较高精度的对准精度，一般会使用双位置对准，如果此时系统需要在转台上评估双位置对准精度，就需要零速判断功能（模拟卫星的定位数据）。

零速判断

本节实现参考文献为《不同惯导系统零速检测算法的性能分析》
常用的零速检测方法为广义似然比检测（GLRT）、加速度计测量方差（MV）、加速度计量测幅值检测（MAG）和角速度量测能量检测（ARE）。

广义似然比检测（GLRT）

广义似然比检测表达式如下：
$$\mathrm{T}(z_n^a,z_n^{\omega })=\frac{1}{W}\sum _{k=n}^{n+W-1}\left(\frac{1}{{\sigma }_a^2}{\Vert y_k^a-g\frac{{\bar{y}}_n^a}{\Vert {\bar{y}}_n^a\Vert }\Vert }^2+\frac{1}{{\sigma }_{\omega }^2}{\Vert y_n^{\omega }\Vert }^2\right)$$
W为采样点数，其中 $z_n^a$和$z_n^{\omega }$位n时刻到n+W-1时刻的观测量，$y_k^a$为三轴加速度值，$y_n^{\omega }$为三轴角速度。
代码如下：

#define ZERO_VELOCITY_WINDOW_SIZE 100	//零速修点加窗点数

typedef struct
{
	double imu_data[ZERO_VELOCITY_WINDOW_SIZE][6];	//IMU数据
	int imu_data_index;								//IMU数据现在的下标
	int imu_data_flag;								//IMU数据慢标志
	double zero_velocity_g;							//零速时钟的加速度
	
	double sigma2_g;
	double sigma2_a;

	double t_glrt;	//广义似然比检测(GLRT)
	
	double threshold;
	int threshold_count;
	int flag_zupt;

}ZEROS_VELOCITY_DAYA;


ZEROS_VELOCITY_DAYA zeros_verocity_data;

void zero_velocity_detector_init(void)
{

	memset(&zeros_verocity_data, 0, sizeof(zeros_verocity_data));

	zeros_verocity_data.imu_data_flag = 0;

	zeros_verocity_data.zero_velocity_g = 9.8014068512546206;

	zeros_verocity_data.sigma2_g = (10.0/3600.0*3.1415926535898 / 180.0)*(10.0 / 3600.0*3.1415926535898 / 180.0);

	zeros_verocity_data.sigma2_a = 0.001*0.001;

	zeros_verocity_data.threshold = 23000;		//门限
	zeros_verocity_data.threshold_count = 100;
}


//广义似然比检测(GLRT)
double zeros_velocity_glrt_cal(void)
{
	int i = 0;
	double ya[3] = { 0 };
	double ya_norm;
	double temp[3];
	double temp2[3];
	double rs;


	ya[0] = 0.0;
	ya[1] = 0.0;
	ya[2] = 0.0;

	//先求加速度的平均值
	for (i = 0;i < ZERO_VELOCITY_WINDOW_SIZE;i++)
	{
		ya[0] = ya[0] + zeros_verocity_data.imu_data[i][0];
		ya[1] = ya[1] + zeros_verocity_data.imu_data[i][1];
		ya[2] = ya[2] + zeros_verocity_data.imu_data[i][2];
	}

	ya[0] = ya[0] / (ZERO_VELOCITY_WINDOW_SIZE*1.0);
	ya[1] = ya[1] / (ZERO_VELOCITY_WINDOW_SIZE*1.0);
	ya[2] = ya[2] / (ZERO_VELOCITY_WINDOW_SIZE*1.0);

	ya_norm = sqrt(ya[0] * ya[0] + ya[1] * ya[1] + ya[2] * ya[2]);

	ya[0] = ya[0] / ya_norm;
	ya[1] = ya[1] / ya_norm;
	ya[2] = ya[2] / ya_norm;

	temp[0] = zeros_verocity_data.zero_velocity_g*ya[0];
	temp[1] = zeros_verocity_data.zero_velocity_g*ya[1];
	temp[2] = zeros_verocity_data.zero_velocity_g*ya[2];

	rs = 0.0;
	for (i = 0;i < ZERO_VELOCITY_WINDOW_SIZE;i++)
	{
		//目前值-重力加速度*求模的均值
		temp2[0] = zeros_verocity_data.imu_data[i][0] - temp[0];
		temp2[1] = zeros_verocity_data.imu_data[i][1] - temp[1];
		temp2[2] = zeros_verocity_data.imu_data[i][2] - temp[2];

		rs = rs + 
			(zeros_verocity_data.imu_data[i][3] * zeros_verocity_data.imu_data[i][3] + 
			 zeros_verocity_data.imu_data[i][4] * zeros_verocity_data.imu_data[i][4] + 
			 zeros_verocity_data.imu_data[i][5] * zeros_verocity_data.imu_data[i][5]) / zeros_verocity_data.sigma2_g +
			(temp2[0] * temp2[0] + temp2[1] * temp2[1] + temp2[2] * temp2[2]) / zeros_verocity_data.sigma2_a;
	}

	rs = rs / (ZERO_VELOCITY_WINDOW_SIZE*1.0);

	return rs;
}


//最大似然估计
void zero_velocity_detector(double wib[3], double fb[3])
{
	static int zero_velocity_count = 0;

	double t_glrt = 0.0;
	double t_mv = 0.0;
	double t_mag = 0.0;
	double t_are = 0.0;


	//先加速度计，再是陀螺。
	zeros_verocity_data.imu_data[zeros_verocity_data.imu_data_index][0] = fb[0];
	zeros_verocity_data.imu_data[zeros_verocity_data.imu_data_index][1] = fb[1];
	zeros_verocity_data.imu_data[zeros_verocity_data.imu_data_index][2] = fb[2];

	zeros_verocity_data.imu_data[zeros_verocity_data.imu_data_index][3] = wib[0];
	zeros_verocity_data.imu_data[zeros_verocity_data.imu_data_index][4] = wib[1];
	zeros_verocity_data.imu_data[zeros_verocity_data.imu_data_index][5] = wib[2];

	zeros_verocity_data.imu_data_index = zeros_verocity_data.imu_data_index + 1;
	if (zeros_verocity_data.imu_data_index >= ZERO_VELOCITY_WINDOW_SIZE)
	{
		zeros_verocity_data.imu_data_index = 0;
		zeros_verocity_data.imu_data_flag = 1;
	}

	//内部已经有数据
	if (zeros_verocity_data.imu_data_flag == 1)
	{

		//广义似然比检测(GLRT)
		t_glrt = zeros_velocity_glrt_cal();
	}

	if (t_glrt < zeros_verocity_data.threshold)
	{
		zero_velocity_count++;
	}
	else
	{
		zero_velocity_count = 0;
	}

	if (zero_velocity_count > zeros_verocity_data.threshold_count)
	{
		zeros_verocity_data.flag_zupt = 1;
	}
	else
	{
		zeros_verocity_data.flag_zupt = 0;
	}

	zeros_verocity_data.t_glrt = t_glrt;
}

零速判断结果

经过处理跑车数据，该判断算法都大部分都能识别出零速，在但是在一些情况下还是存在误判的情况（应该还有优化空间），所以这里对于该算法运用场景目前我仅用与转台上对准，或者静态对准上。