悬停油门的参数估计方法

估计悬停油门，实际上就是完成油门值与飞机加速度之间的映射关系，准确的估计值可以让飞行控制更为准确。在PX4中，对悬停油门的估计方法采用的是扩展卡尔曼滤波，Fast_drone_250 的px4_ctrl中采用的是衰减记忆迭代递推最小二乘法。这两种方法都对悬停油门参数可以进行很好的估计。这里，我对两种方法都展开分析与记录。

扩展卡尔曼滤波

首先我们要建立起模型方程：

u = t h r u s t , x = h o v e r _ t h r , y = a _ z , z = a _ z _ m e a s u r e u=thrust, \quad x=hover\_thr, \quad y = a\_z ,\quad z = a\_z\_measure u=thrust,x=hover_thr,y=a_z,z=a_z_measure 加速度是在ENU下状态估计出来的运动加速度与重力加速度无关，不是imu的原始值。

_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);
//a_z本来是NED下的来着，这里他又转换了一下。变成了我们更习惯的z轴向上！

估计的油门值是在world-fixed NED下的z轴分量。这样一来就对了，估计出来的悬停油门就是飞机一动不动的油门值，其作用就是把油门与加速度建立准确的映射关系，与px4ctrl里的结果是一致！之前一直弄错了果然坐标系的确定非常重要也非常让人容易晕头！

# Local position setpoint in NED frame
# setting something to NaN means the state should not be controlleduint64 timestamp # time since system start (microseconds)float32 x       # in meters NED
float32 y       # in meters NED
float32 z       # in meters NED
float32 yaw     # in radians NED -PI..+PI
float32 yawspeed    # in radians/sec
float32 vx      # in meters/sec
float32 vy      # in meters/sec
float32 vz      # in meters/sec
float32[3] acceleration # in meters/sec^2
float32[3] jerk # in meters/sec^3
float32[3] thrust   # normalized thrust vector in NED

x k = x k − 1 + w k − 1 y k = g u k x k − g z k = y k + v k \begin{equation} \begin{aligned} x_k=x_{k-1}+w_{k-1} \\ y_k =\frac {g \ u_k}{x_k}-g \\ z_k = y_k+v_k \end{aligned} \end{equation} xk=xk−1+wk−1yk=xkg uk−gzk=yk+vk

其中噪声分布为, 过程噪声： E ( w k ) = 0 , E ( w k w k T ) = Q k E(w_k)=0, E(w_k w_k^T)=Q_k E(wk)=0,E(wkwkT)=Qk , 量测噪声： E ( v k ) = 0 , E ( v k v k T ) = R k E(v_k)=0, E(v_k v_k^T)=R_k E(vk)=0,E(vkvkT)=Rk

在非线性处做一阶线性展开，可得：
C k = − g u k x k 2 Φ k − 1 = 1 C_k=- \frac {gu_k }{x_k^2}\\ \Phi_{k-1}=1 Ck=−xk2gukΦk−1=1
根据上式可得：
x k ∣ k − 1 = x k − 1 z k ∣ k − 1 = g u k − 1 x k − 1 − g P k ∣ k − 1 = P k − 1 + Q k − 1 K k = P k ∣ k − 1 C k ( C k P k ∣ k − 1 C k + R k ) − 1 x k = x k ∣ k − 1 + K k ( z k − z k ∣ k − 1 ) P k = ( 1 − K k C k ) P k ∣ k − 1 x_{k|k-1}=x_{k-1} \\ z_{k|k-1}=\frac {g \ u_{k-1}}{x_{k-1}}-g \\ P_{k|k-1}=P_{k-1}+Q_{k-1} \\ K_{k}=P_{k|k-1}C_k(C_kP_{k|k-1}C_k+R_k)^{-1} \\ x_k=x_{k|k-1}+K_k(z_k-z_{k|k-1}) \\ P_k=(1-K_kC_k)P_{k|k-1} xk∣k−1=xk−1zk∣k−1=xk−1g uk−1−gPk∣k−1=Pk−1+Qk−1Kk=Pk∣k−1Ck(CkPk∣k−1Ck+Rk)−1xk=xk∣k−1+Kk(zk−zk∣k−1)Pk=(1−KkCk)Pk∣k−1
这些式子也就是估计器代码种的公式，不过PX4里面还有一部分是一个低通滤波和更新传感器量测噪声的，这个我不太清楚怎么来的。总的来说，通过这个扩展卡尔曼滤波，我们就可以估计出无人机的实时的悬停油门。

衰减记忆迭代递推最小二乘法

px4ctrl里的模型如下。

这里不一样的是，这里的 a z a_z az 是体轴系（body-fixed ENU）下的加速度计测量值（不是加速度值！），thr是实际的油门值（即机体系下）
a z = t h r 2 a c c ∗ t h r a_z = thr2acc* thr az=thr2acc∗thr
这样的局部线化模型就油门值与机体z轴加速度映射关系完成里建立！eg : 当 t h r = t h r _ h o v e r thr=thr\_hover thr=thr_hover时， a z = g a_z = g az=g

该模型参数估计采用了衰减记忆迭代递推最小二乘，其公式和该方法的标准形式一模一样，拿着相关资料一对便知，这里不再展开。

这里顺便一提，我觉得PX4和px4ctrl里面的油门值都不对

PX4里那个body_z，我觉得应该是（-ax_sp, -ay_sp, -az_sp+g) 而不是（-ax_sp, -ay_sp, g)

Fast_drone_250中px4ctrl里那个acc_sp(2)/thr2acc，我觉得应该是 acc_sp.norm()/thr2acc

这样的理解来源于我觉得自己对悬停油门的理解是没有问题的。对于这两个地方，我也都做了修改和仿真测试，控制效果和精度并看不出来有明显提升，可能是机动动作比较小，但是至少是没有变坏的。