悬停油门的参数估计方法
估计悬停油门，实际上就是完成油门值与飞机加速度之间的映射关系，准确的估计值可以让飞行控制更为准确。在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 controlled
uint64 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
这样的理解来源于我觉得自己对悬停油门的理解是没有问题的。对于这两个地方，我也都做了修改和仿真测试，控制效果和精度并看不出来有明显提升，可能是机动动作比较小，但是至少是没有变坏的。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
