IMU | 惯导模块指标参数与标定

组合定位模块初始化

组合定位过程中对IMU/GNSS等模块进行初始化，主要针对质量参数进行估计

【停车判断】

• 空旷环境：
  可以依赖GPS、轮速辅助做停车判断，动态GPS测速精度达到mm级别，通过GPS、轮速判断停车比单纯依靠IMU判断要准确；
• 遮挡环境：
  如果有轮速可以辅助做停车判断，则优先使用轮速做判断；

【初始化策略】

• 静态初始化：
  指的是进行姿态（水平角）、位置、速度、航向初始值获取的过程；

IMU初始化：除了上面提到的姿态(水平角)的初始化外，还有固定零偏、噪声的估计(这两个值在车辆非静止模式下比较难测出)

• 动态初始化：
  指的是跑车过程中通过GPS信号对IMU模块的相对安装位置进行估计，包括安装偏差角等，完成自对准操作；

【初始化现状】
车辆运行过程中，重启设备，保持车辆动态不停车，车辆不能完成静态初始化；

【存在问题】
IMU未实现静态初始化，则不能有效的对IMU固定零偏、噪声进行估计，所以该状态在无轮速的遮挡环境下容易出现推算异常的问题（出错概率完全取决于IMU的可靠程度）

初始化方案和用户的实际使用场景、客户的需求相关，追求便利的一定程度上会损失估计精度；

IMU模块参数定义代码参考

/**
 * @GNSS_INS.h
 * @berif: GNSS_INS variable define.
 */

#ifndef _GNSS_INS_H
#define _GNSS_INS_H

/* It's valid in 2037 years. */
typedef struct { /* time struct */
	long int time; /* time (s) expressed by standard time_t */
	double sec; /* fraction of second under 1 s */
} gtime_t;

namespace libGNSS_INS
{
	typedef struct
	{
		gtime_t time; //second
		double latitude;//deg
		double longitude;//deg
		double altitude;//m
		double heading;//deg

		double std_latitude;
		double std_longitude;
		double std_altitude;

		int position_status;//fixed float single...
		int heading_status;
	}GNSS_Data;

	typedef struct
	{
		gtime_t time;
		double gyro[3]; //deg/s
		double accel[3];//m/s2
	}IMU_Data;

	typedef struct
	{
		gtime_t time;

		double latitude; //deg
		double longitude;//deg
		double altitude;//m
		double heading;

		double roll;//deg
		double pitch;
		double yaw;

		double std_latitude;
		double std_longitude;
		double std_altitude;
		double std_heading;

//define the velocity or not.

		double std_roll;
		double std_pitch;
		double std_yaw;

		int position_status;//fixed float single...
		int heading_status;
	}GNSSINS_Data;

	typedef struct
	{
		//Algorithm  define some error status
		int accel_error;
		int gyro_error;
		int time_error;
		int arm_error;
	}Error_Flag;

	typedef struct
	{
		double X;
		double Y;
		double Z;

		double estimate_X;
		double estimate_Y;
		double estimate_Z;

//define the rotation installation deviation angle.
//define the vehicle arm lever.
//define output position coordinate.
	}Antenna_Arm;

	typedef struct
	{
		//Output Rate  HZ
		double Output_Rate;
		//Output Sample Rate (max)
		double OutputSample_Rate;

		//Accel. Range
		double Accel_Range;
		//Accel. In-Run Bias Stability (typ) g
		double AccelBias_Stability;
		//Accelerometer Velocity Random Walk (typ)  (m/s)/rthr
		double AccelVelocity_RandomWalk[3];
		//Noise Density (typ) g/rtHz
		double AccelNoise_Density;
		//Accelerometer Output Total Noise (typ) g
		double Accel_TotalNoise;
		//0G Offset Tempco (typ)  ppm/掳C
		double Accel_Tempco;
		//Accelerometer Non-Linearity (typ) % FSR
		double Accel_NonLinearity;
		//Accelerometer Axis to Axis Alignment (typ)
		double AccelAxistoAxis_Alignment;
		//Calibrated Temp Range
		double Accel_CalibratedTemperatureRange;

		//Gyro Input Range +/- (min) deg/s
		double GyroInput_Range;
		//Gyro In-Run Bias Stability (typ) deg/hr
		double GyroBias_Stability[3];
		//Gyro Angular Random Walk (typ) deg/rthr
		double GyroAngular_RandomWalk[3];
		//Gyro Noise Density (typ) (deg/s)/rtHz
		double GyroNoise_Density;
		//Gyro Linear G (typ) (deg/s)/g
		double GyroLinear_G;
		//Gyro Axis to Axis Alignment (typ) deg
		double GyroAxistoAxis_Alignment;
		//Gyro Bias Repeatability
		double GyroBias_Repeatability;
	}IMU_Model;

	class GNSS_INS
	{
	public:
		GNSS_INS();
		~GNSS_INS() = default;

		int SetIMUPar(IMU_Model *imu_par);
		void SetAntenna_Arm(Antenna_Arm arm);
		/**/
		int Calibration(GNSS_Data *gnss_data, IMU_Data *imu_data, Error_Flag* flag);

		int Init(GNSS_Data *gnss_data, IMU_Data *imu_data, Error_Flag* flag);

		void GetAntenna_Arm(Antenna_Arm* arm);

		// GNSS_Data 1/5HZ  
		// IMU_Data   100/200 HZ
		// GNSSINS_Data depend on IMU_Data 
		int Integration(GNSS_Data *gnss_data, IMU_Data *imu_data, GNSSINS_Data *integrate_result,Error_Flag* flag);

	private:
		//arm Parameters
		Antenna_Arm antenna_arm_;
		//IMU_Model
		IMU_Model imumode_;

		//Algorithm  add some variable
		double last_time_,cur_time_,dt_;
	};

}
#endif

/**
 * @GNSS_INS.c
 * @berif: GNSS_INS function define.
 */
#include <string.h>
#include "GNSS_INS.h"

namespace libGNSS_INS
{
	int GNSS_INS::Init(IMU_Model *init_par)
	{
		memcpy((unsigned char *)(&imumode_),init_par,sizeof(IMU_Model));
		return 0;
	}

	void GNSS_INS::GetAntenna_Arm(Antenna_Arm* arm)
	{
		memcpy((unsigned char *)(arm),&Antenna_Arm_,sizeof(Antenna_Arm));
	}

	void GNSS_INS::SetAntenna_Arm(Antenna_Arm arm)
	{
		memcpy((unsigned char *)(&Antenna_Arm_),&arm,sizeof(Antenna_Arm));
	}

	void GNSS_INS::SetCalibrationFlag(bool flag)
	{
		calibrationFlag_ = flag;
	}

	int Integration(GNSS_Data *gnss_data, IMU_Data *imu_data, GNSSINS_Data *integrate_result,Error_Flag* flag)
	{

	}
}