【code review】2、关于高度的估计过程

0、前言

在定高模式中，飞控需要有当前高度的信息，也就是z的position信息，进行Z轴的位置环控制；那么这个Z轴的位置信息是怎么来的呢？本文为在解读wukong FPV源码中Z轴高度估计的过程的笔记；

1、更新预估器的中气压计数据

（1）估计器数据结构

在估计器中，有三个成员变量，下面是估计器的数据结构：

typedef struct 
{
    // Data sources
    navPositionEstimatorBARO_t  baro;
    navPosisitonEstimatorIMU_t  imu;

    // Estimate
    posEstimatorEst_t  est;
	
} posEstimator_t;

上面代码块中，源数据是气压数据和imu数据，最终要合成est中的最终估计值，而飞行的state就是由est信息发布而来；

（2）code——气压计数据更新

//更新气压计数据
static void updateBaroTopic(float newBaroAlt)
{
	if (baroIsCalibrationComplete()) 
	{
		posEstimator.baro.alt = newBaroAlt;
	}
	else
	{
		posEstimator.baro.alt = 0.0f;
	}
}

上述代码块就是将传感器中气压计的数据赋值进去；

2、更新预估器的加速度数据

（1）估计器中imu的结构体

typedef struct 
{
    Axis3f     accelNEU;
    Axis3f     accelBias;
    bool       gravityCalibrationComplete;
} navPosisitonEstimatorIMU_t;

上述code块中，imu的数据只有NEU上的加速度数据和加速度偏置，还有一个重力方向的校验标志位；

（2）code——更新NEU坐标系的加速度

//更新NEU坐标系的加速度
//accBF：为机体坐标系加速度
static void updateIMUTopic(const Axis3f accBF)
{
    static float calibratedGravityCMSS = GRAVITY_CMSS;
    static u32 gravityCalibrationTimeout = 0;
	Axis3f accelCMSS;

	//机体坐标系的加速度转为（cm/ss）
	accelCMSS.x = accBF.x * GRAVITY_CMSS;
	accelCMSS.y = accBF.y * GRAVITY_CMSS;
	accelCMSS.z = accBF.z * GRAVITY_CMSS;

	//去除加速度偏置
	accelCMSS.x -= posEstimator.imu.accelBias.x;
	accelCMSS.y -= posEstimator.imu.accelBias.y;
	accelCMSS.z -= posEstimator.imu.accelBias.z;

	//旋转机体坐标系加速度到NEU坐标系
	imuTransformVectorBodyToEarth(&accelCMSS);
	
	//在水平状态校准重力方向的加速度零偏
	if (!posEstimator.imu.gravityCalibrationComplete && STATE(SMALL_ANGLE)) 
	{
		//慢慢收敛校准重力零偏
		const float gravityOffsetError = accelCMSS.z - calibratedGravityCMSS;
		calibratedGravityCMSS += gravityOffsetError * 0.0025f;

		if (ABS(gravityOffsetError) < 5)//误差要小于5cm/ss
		{
			if ((getSysTickCnt() - gravityCalibrationTimeout) > 250) 
			{
				posEstimator.imu.gravityCalibrationComplete = true;
			}
		}
		else 
		{
			gravityCalibrationTimeout = getSysTickCnt();
		}
	}
	
	//NEU坐标系加速度处理
	if (posEstimator.imu.gravityCalibrationComplete) 
	{
		accelCMSS.z -= calibratedGravityCMSS;//去除重力
		for (int axis = 0; axis < 3; axis++)
		{
			applyDeadband(accelCMSS.axis[axis], 4);//去除4(cm/ss)死区
			posEstimator.imu.accelNEU.axis[axis] += (accelCMSS.axis[axis] - posEstimator.imu.accelNEU.axis[axis]) * 0.3f;//一阶低通
		}
	}
	else 
	{
		posEstimator.imu.accelNEU.x = 0;
		posEstimator.imu.accelNEU.y = 0;
		posEstimator.imu.accelNEU.z = 0;
	

上述code块中，输入的是imu的原始加速度数据；

（3）解读加速度更新步骤

1. 首先将加速度进行单位的转换；
2. 将转换后的加速度数据减去估计器中加速度的偏置值， 这个偏置值是怎么来的，比如Z轴方向的偏置值是气压计的微分得到的；
3. 进一步处理观测到的加速度数据，就是将其转换到ENU坐标系；
4. 在水平状态校准重力方向的加速度零偏，这一步没看明白；但是实际值跑一次；
   其中一个重要的作用是计算重力产生的零偏；
5. NEU坐标系加速度处理，这个是最重要的处理：

	//NEU坐标系加速度处理
	if (posEstimator.imu.gravityCalibrationComplete) 
	{
		accelCMSS.z -= calibratedGravityCMSS;//去除重力
		for (int axis = 0; axis < 3; axis++)
		{
			applyDeadband(accelCMSS.axis[axis], 4);//去除4(cm/ss)死区
			posEstimator.imu.accelNEU.axis[axis] += (accelCMSS.axis[axis] - posEstimator.imu.accelNEU.axis[axis]) * 0.3f;//一阶低通
		}
	}
	else 
	{
		posEstimator.imu.accelNEU.x = 0;
		posEstimator.imu.accelNEU.y = 0;
		posEstimator.imu.accelNEU.z = 0;
	}

这里将code再贴一次，最终得到了估计器中imu的加速度值；其中Z方向的加速度观测值数据再减去重力的偏置之后；最终得到：
z加速度估计值 = z加速度估计值 + (z加速度观测值 - z加速度的估计值) 0.3*

3、速度预估和位置预估

终于到速度和位置的估计了，这一步就是通过之前的估计得到的imu的加速度数据，得到飞机的当前位置和速度信息；

（1）code——速度预估和位置预估

//速度预估和位置预估
static void updateEstimatedTopic(float dt)
{
	//使用加速度预估位移和速度
	posAndVelocityPredict(Z, dt, posEstimator.imu.accelNEU.z);
	posAndVelocityPredict(Y, dt, posEstimator.imu.accelNEU.y);
	posAndVelocityPredict(X, dt, posEstimator.imu.accelNEU.x);
	
    //加速度偏置值
    const bool updateAccBias = (W_ACC_BIAS > 0);
    Axis3f accelBiasCorr = { { 0, 0, 0} };

	//使用气压计高度误差修正预估的位移和速度（Z轴）
	const float baroResidual =   posEstimator.baro.alt - posEstimator.est.pos.z;
	posAndVelocityCorrect(Z, dt, baroResidual, W_Z_BARO_P);
	if (updateAccBias) 
	{
		accelBiasCorr.z -= baroResidual * sq(W_Z_BARO_P);
	}
	
	//修正加速度偏置值
    if (updateAccBias) 
	{
        const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
        if (accelBiasCorrMagnitudeSq < sq(GRAVITY_CMSS * 0.25f))//偏置小于0.25G时可以修正 
		{
			//将加速度偏置值由世界坐标系转换为机体坐标系
            imuTransformVectorEarthToBody(&accelBiasCorr);
			
            posEstimator.imu.accelBias.x += accelBiasCorr.x * W_ACC_BIAS * dt;
            posEstimator.imu.accelBias.y += accelBiasCorr.y * W_ACC_BIAS * dt;
            posEstimator.imu.accelBias.z += accelBiasCorr.z * W_ACC_BIAS * dt;
        }
    }
}

（2）解读速度预估和位置预估的步骤

1. 首先就是用预估器中imu数据预估速度和位置

static void posAndVelocityPredict(int axis, float dt, float acc)
{
    posEstimator.est.pos.axis[axis] += posEstimator.est.vel.axis[axis] * dt + acc * dt * dt / 2.0f;
    posEstimator.est.vel.axis[axis] += acc * dt;
}

上述code非常容易理解，其实就是
x = x0 + V0 *dt + 1/2 * a t^2
v = at + v0
如此就可以知道X，Y,Z方向的速度和位置信息了吗，继续往下面看；

2. //使用气压计高度误差修正预估的位移和速度（Z轴）
   此处就是有一个观测到的高度数据，也就是气压计的数据，另一个是估计的数据，也就是预估器中的z轴方向的速度和位置信息；
   那么就可以做一下处理了；

//误差修正预估的位移和速度
static void posAndVelocityCorrect(int axis, float dt, float e, float w)
{
    float ewdt = e * w * dt;
    posEstimator.est.pos.axis[axis] += ewdt;
    posEstimator.est.vel.axis[axis] += w * ewdt;
}

其中的e是Z位置观测值与估计的Z位置估计值的差；
总结就是：
Z位置估计值 = Z位置估计值+ （ Z位置观测值- Z位置估计值） 权重*dt*
这个操作就和上面的
z加速度估计值 = z加速度估计值 + (z加速度观测值 - z加速度的估计值) 0.3
非常相似；但是为什么要乘上一个dt呢；
3. //修正加速度偏置值

4、速度和位置的发布

（1）code——速度和位置的发布

//发布预估位置和速度
static void publishEstimatedTopic(state_t *state)
{
	static u32 publishTime;
	
	//更新世界坐标系的加速度
	state->acc.x = posEstimator.imu.accelNEU.x;
	state->acc.y = posEstimator.imu.accelNEU.y;
	state->acc.z = posEstimator.imu.accelNEU.z;
	
	//更新估计的位置和速度（10ms->100Hz）
	if ((getSysTickCnt() - publishTime) >= 10)
	{
		state->position.x = posEstimator.est.pos.x;
		state->position.y = posEstimator.est.pos.y;
		state->position.z = posEstimator.est.pos.z;
		
		state->velocity.x = posEstimator.est.vel.x;
		state->velocity.y = posEstimator.est.vel.y;
		state->velocity.z = constrainf(posEstimator.est.vel.z, -150.0f, 150.0f);//限制Z轴的速度为150cm/s
		
		publishTime = getSysTickCnt();
	}
}

上述code就比较好理解了，就是将预估器中得到的速度和位置信息赋值给无人机的state变量，使其参与到控制中去；