PX4_ECL_EKF代码分析2

写在前面

源码版本：1.6.0rc1
源码位置1：Firmware-1.6.0rc1\src\modules\ekf2_main.cpp
源码位置2：Firmware-1.6.0rc1\src\lib\ecl\EKF\

整体框架：

上图PX4的EKF代码框架，PX4的代码由两部分组成，一部分是在modules下的ekf2，另一部分是ecl代码库中EKF部分。

第一部分实现数据的订阅（subscribe）、整理、储存、经过处理的数据发布（publish）。
第二部分实现数据的处理。

第二部分：

bool Ekf::update()
{
	if (!_filter_initialised) {
		_filter_initialised = initialiseFilter();

		if (!_filter_initialised) {
			return false;
		}
	}
	// Only run the filter if IMU data in the buffer has been updated
	if (_imu_updated) {
		// perform state and covariance prediction for the main filter
		predictState();
		predictCovariance();
		
		// control fusion of observation data
		controlFusionModes();

		// run a separate filter for terrain estimation
		runTerrainEstimator();

	}
	// the output observer always runs
	calculateOutputStates();

	// check for NaN or inf on attitude states
	if (!ISFINITE(_state.quat_nominal(0)) || !ISFINITE(_output_new.quat_nominal(0))) {
		return false;
	}
	// We don't have valid data to output until tilt and yaw alignment is complete
	return _control_status.flags.tilt_align && _control_status.flags.yaw_align;
}

这个就是整个EKF代码的核心函数了，分别由几个函数组成，各自完成不同的功能。

在首次进入update()这个函数的时候，系统会对变量进行初始化initialiseFilter();这个函数只执行一次，跟进：

bool Ekf::initialiseFilter()
{
	// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
    //继续累积测量，直到有至少10个样品为所需的传感器
	// Sum the IMU delta angle measurements
	imuSample imu_init = _imu_buffer.get_newest();
	_delVel_sum += imu_init.delta_vel;
	// Sum the magnetometer measurements
	//对磁强计的测量值求和
	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
		if ((_mag_counter == 0) && (_mag_sample_delayed.time_us != 0)) {
			// initialise the counter when we start getting data from the buffer
			_mag_counter = 1;
		} else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
			// increment the sample count and apply a LPF to the measurement
			_mag_counter ++;

			// don't start using data until we can be certain all bad initial data has been flushed
			//在确定所有坏的初始数据都已被清除之前，不要开始使用数据
			if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
				// initialise filter states
				_mag_filt_state = _mag_sample_delayed.mag;

			} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
				// noise filter the data
				_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
			}
		}
	}

磁力计数据出栈

	// set the default height source from the adjustable parameter
	if (_hgt_counter == 0) {
		_primary_hgt_source = _params.vdist_sensor_type;
	}

上面代码，选择高度源，可以来自于气压计，GPG，视觉、（毫米波）超声波等。下面开始选择高度源

	// accumulate enough height measurements to be confident in the qulaity of the data
	//积累足够的高度测量值以对保证数据的质量
	if (_primary_hgt_source == VDIST_SENSOR_RANGE) {//使用超声波
		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
			if ((_hgt_counter == 0) && (_range_sample_delayed.time_us != 0)) {
				// initialise the counter height fusion method when we start getting data from the buffer
				_control_status.flags.baro_hgt = false;
				_control_status.flags.gps_hgt = false;
				_control_status.flags.rng_hgt = true;//使能超声波标志位
				_control_status.flags.ev_hgt = false;
				_hgt_counter = 1;

			} else if ((_hgt_counter != 0) && (_range_sample_delayed.time_us != 0)) {
				// increment the sample count and apply a LPF to the measurement
				_hgt_counter ++;
				// don't start using data until we can be certain all bad initial data has been flushed
				if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
					// initialise filter states
					_rng_filt_state = _range_sample_delayed.rng;

				} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
					// noise filter the data
					_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
				}
			}
		}//超声波数据出栈
	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {//使用气压计或者GPS
		// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
		//如果user参数指定高度使用GPS，我们首先使用气压高度，然后切换到GPS
		// later when it passes checks.
		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
			if ((_hgt_counter == 0) && (_baro_sample_delayed.time_us != 0)) {
				// initialise the counter and height fusion method when we start getting data from the buffer
				_control_status.flags.baro_hgt = true;//使用气压计
				_control_status.flags.gps_hgt = false;
				_control_status.flags.rng_hgt = false;
				_hgt_counter = 1;

			} else if ((_hgt_counter != 0) && (_baro_sample_delayed.time_us != 0)) {
				// increment the sample count and apply a LPF to the measurement
				_hgt_counter ++;

				// don't start using data until we can be certain all bad initial data has been flushed
				if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
					// initialise filter states
					_baro_hgt_offset = _baro_sample_delayed.hgt;

				} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
					// noise filter the data
					_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
				}
			}
		}
	} else if (_primary_hgt_source == VDIST_SENSOR_EV) {//使用视觉传感器
		_hgt_counter = _ev_counter;
	} else {
		return false;
	}

	// check to see if we have enough measurements and return false if not
	bool hgt_count_fail = _hgt_counter <= 2 * _obs_buffer_length;
	bool mag_count_fail = _mag_counter <= 2 * _obs_buffer_length;
	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2 * _obs_buffer_length);
	
		if (hgt_count_fail || mag_count_fail || ev_count_fail) {
		return false;
	} else {
		// reset variables that are shared with post alignment GPS checks重置与后校准GPS检查共享的变量
		_gps_drift_velD = 0.0f;
		_gps_alt_ref = 0.0f;
		// Zero all of the states
		_state.vel.setZero();
		_state.pos.setZero();
		_state.gyro_bias.setZero();
		_state.accel_bias.setZero();
		_state.mag_I.setZero();
		_state.mag_B.setZero();
		_state.wind_vel.setZero();

检测是否有足够的测量数据，如果没有则返回 false，复位状态

// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
//假设飞行器是静态的，从速度矢量得到初始横滚和俯仰估计
	float pitch = 0.0f;
	float roll = 0.0f;
	if (_delVel_sum.norm() > 0.001f) {
		_delVel_sum.normalize();
		pitch = asinf(_delVel_sum(0));
		roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
	} else {
		return false;
	}

// calculate initial tilt alignment
		matrix::Euler<float> euler_init(roll, pitch, 0.0f);
		_state.quat_nominal = Quaternion(euler_init);
		_output_new.quat_nominal = _state.quat_nominal;
		_R_to_earth = quat_to_invrotmat(_state.quat_nominal);

根据上面得到的初始pitch 、roll计算初始倾斜角，并更新机体系到导航系的旋转矩阵。

	Vector3f mag_init = _mag_filt_state;
	
	// calculate the initial magnetic field and yaw alignment
	_control_status.flags.yaw_align = resetMagHeading(mag_init);

磁力计数值初始化，进行偏航校准，后面的超声波作为高度源的代码没有粘贴直接跳过。

		// initialise the state covariance matrix
		initialiseCovariance();

协方差初始化函数，进去看一下：

void Ekf::initialiseCovariance()
{
	for (unsigned i = 0; i < _k_num_states; i++) {//首先是协方差矩阵内容清零，以便赋值
		for (unsigned j = 0; j < _k_num_states; j++) {
			P[i][j] = 0.0f;
		}
	}
	// calculate average prediction time step in sec//计算平均预测时间 单位 秒
	float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;

	// define the initial angle uncertainty as variances for a rotation vector
	//定义初始角度不确定度为旋转矢量的方差
	Vector3f rot_vec_var;
	rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(_params.initial_tilt_err);

	//使用旋转向量方差初始化四元数协方差
	initialiseQuatCovariances(rot_vec_var);

	// velocity
	P[4][4] = sq(fmaxf(_params.gps_vel_noise, 0.01f));
	P[5][5] = P[4][4];
	P[6][6] = sq(1.5f) * P[4][4];

	// position
	P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
	P[8][8] = P[7][7];
	if (_control_status.flags.rng_hgt) {//使用超声波 （这里不使用）
		P[9][9] = sq(fmaxf(_params.range_noise, 0.01f));
	} else if (_control_status.flags.gps_hgt) {//用GPS
		float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
		float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
		P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
	} else {
		P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
	}

	// gyro bias
	P[10][10] = sq(_params.switch_on_gyro_bias * dt);
	P[11][11] = P[10][10];
	P[12][12] = P[10][10];

	// accel bias
	_prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt);
	_prev_dvel_bias_var(1) = P[14][14] = P[13][13];
	_prev_dvel_bias_var(2) = P[15][15] = P[13][13];
	
	// earth frame and body frame magnetic field
	// set to observation variance
	for (uint8_t index=16; index <= 21; index ++) {
		P[index][index] = sq(_params.mag_noise);
	}

	// wind
	P[22][22] = 1.0f;
	P[23][23] = 1.0f;
}

initialiseQuatCovariances(rot_vec_var);这个函数可以到matlab模型里面可以找到 ，编译后会生成此函数，matlab路径：

Firmware-1.6.0rc1\src\lib\ecl\EKF\matlab\Inertial Nav EKF\QuatErrTransferEquations.m

生成的.c文件也在同路径下。

	//	重置基本的融合超时计数器
		_time_last_hgt_fuse = _time_last_imu;
		_time_last_pos_fuse = _time_last_imu;
		_time_last_vel_fuse = _time_last_imu;
		_time_last_hagl_fuse = _time_last_imu;
		_time_last_of_fuse = _time_last_imu;
	// reset the output predictor state history to match the EKF initial values
	//重置输出预测器状态历史以匹配EKF初始值
		alignOutputFilter();
		return true;
	}
}//end bool Ekf::initialiseFilter();

void Ekf::alignOutputFilter()
{
	// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
	//在EKF融合时间层计算输出和EKF四元数之间的delta四元数
	Quaternion quat_inv = _state.quat_nominal.inversed();
	Quaternion q_delta =  _output_sample_delayed.quat_nominal * quat_inv;
	q_delta.normalize();

	// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
	//在EKF融合时间层计算输出和EKF之间的速度和位置增量
	Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
	Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;

	// loop through the output filter state history and add the deltas
	//循环遍历输出过滤器状态历史记录并添加增量
	outputSample output_states = {};
	unsigned output_length = _output_buffer.get_length();

	for (unsigned i = 0; i < output_length; i++) {
		output_states = _output_buffer.get_from_index(i);
		output_states.quat_nominal *= q_delta;
		output_states.quat_nominal.normalize();
		output_states.vel += vel_delta;
		output_states.pos += pos_delta;
		_output_buffer.push_to_index(i, output_states);
	}
}

以上就是我对ECL库中EKF代码初始化函数的理解，有很多写在了注释里面，也有很多不理解的地方还在学习中，如有理解错误的地方请批评指正，欢迎一起学习填坑PX4，QQ：1103706199