BetaFlight深入传感设计之七：GPS&Baro高度数据融合

传感器数据融合最主要的目的是为了数据的精准。同时也可以通过多个传感数据源来判断和纠正异常数据。

最近在飞BetaFlight的时候，总是感觉高度数据异常，明明在天上飞的，结果高度是负值。另外，BF团队也在考虑在4.4版本逐步引入高精度的GPS Resucre功能，甚至有Return To Home，然后Auto Landing的功能。

所以感觉有必要更深入了解下这方面的机制和误差的来源。

1. 现象

OSD显示的数据显示高度负值，比较明显。视频可以大家看下：

• 【1】BetaFlight Mark4 + GPS Rescure(Mannual Trigger) + 梅灵南路

— 注意观察负值高度，采用视频时间戳。第一次起飞 —
01:24 起飞校准 0米， GPS卫星数量11颗
01:28 起飞[lift off]，出现一个-10米的数据(尴尬)
01:53 持续飞30秒，该飞地是一个茶叶坡地，所以一直在升高，直到30秒OSD显示正值高度
04:06 飞行2分43秒，靠近起飞地点，高度开始出现负值
04:34 飞行3分11秒，降落瞬间，高度显示-12.5米
— 注意起飞校准时刻0米高度，起飞瞬间-10米数据，降落书剑-12.5米。第二次起飞 —
05:31 再次起飞 校准 0米，GPS卫星数量10颗
05:34 起飞[lift off], 出现一个-1米的数据，后续正常攀升(正值)
08:09 飞行2分37秒，接近起飞点，出现负值高度
08:13 飞越公路桥，在起飞点上方5_{6米高度，但是OSD显示-5.4米(尴尬}~)
08:56 再次靠近起飞点，OSD仍然显示负值-3.8米
09:55 重新飞第一次飞行路径，OSD显示高度负值(持续，稳定负值；随实际海拔高度提升，高度逐步回升)
11:41 再次公路桥高度附近飞行，OSD显示高度持续为负
14:04 降落瞬间，OSD显示高度 - 5.4米

• 【2】BetaFlight Mark4 + 梅灵南路 + 无牙崽2晴天

00:10 起飞校准 高度0米，GPS卫星14颗，晴天
00:12 起飞瞬间 高度-2.4米，随后马上正常
05:34 接近起飞点，高度显示0米（波动，随后显示1-2米高度）
05:41 再次出现一个-0.1米的高度（波动，随后显示正常）
05:56 降落瞬间，OSD显示高度0米（非常正常）

• 【3】BetaFlight Mark4 + 善贤地铁站口

00:01 起飞校准 高度0米，GPS卫星16颗，晴天
00:02 起飞瞬间 高度-3.2米，随后正常
01:31 飞行一段时间，目前高度已经过围墙，显然不止2米，但是OSD显示2米
02:12 飞行中，OSD显示高度0米【异常】
03:18 飞过起飞点上方，OSD显示高度-2.2米【异常】，后面全程可以关注，靠近地面的基本都是负值高度
12:39 降落瞬间，高度-3米【异常】

所以，BF 4.3.1固件所使用的GPS+Baro传感器数据融合方法，精度并不理想，甚至出现尴尬的负值。

2. 分析

2.1 程序逻辑

先根据之前研读一些内容，整理下传感高度数据融合的计算逻辑：

【1】BetaFlight模块设计之九：气压计任务分析
【2】BetaFlight模块设计之十四：高度计算任务分析
【3】BetaFlight深入传感设计之一：Baro传感模块
【4】BetaFlight深入传感设计之四：GPS传感模块

• Step1: 上电时，使用数学平均值算法，记录baroGroundAltitude
• Step2: 定时更新气压计高度pressureToAltitude，计算BaroAlt_tmp = pressureToAltitude - baroGroundAltitude
• Step3: 做一个低通滤波baro.BaroAlt = baro.BaroAlt * barometerConfig()->baro_noise_lpf + BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)
• Step4: 为防止上电时刻与解锁时刻高度差异值，解锁时计算高度差baroAltOffset = baroAlt
• Step5: 实际飞行时，气压计高度baroAlt -= baroAltOffset
• Step6: 【传感融合】【第一次GPS高度有效时刻】计算gpsAltOffset = gpsAlt - baroAlt
• Step7: 【传感融合】【GPS高度有效】gpsTrust = 100.0 / gpsSol.hdop //但不超过 0.9, 至少保留10%的气压计高度
• Step8: 【GPS有效性判断】有效判断依据：> altNumSatsGpsUse; 无效回退判断依据：< altNumSatsBaroFallback
• Step9: 【高度计算】

【GPS + baro】estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust)
【仅GPS】 estimatedAltitudeCm = gpsAlt
【仅baro】estimatedAltitudeCm = baroAlt

注：默认GPS高度有效性判断依据。

position_alt_gps_min_sats = 10
Allowed range: 4 - 50

position_alt_baro_fallback_sats = 7
Allowed range: 3 - 49

2.2 GPS精度

DOP是Dilution of Precision的缩写，直译为“精度强弱度”，通常翻译为“精度因子”。精度因子：一种描述纯粹因卫星几何因素对定点精度的影响，指出了在测量时被跟踪卫星几何结构上的强度。包括：

• GDOP (Geometrical)：包括经度，纬度,高程和时间等因子，称为几何精度因子。
• PDOP (Positional)：范围：0.5 - 99.9，包括经度,纬度和高程等因子，称为三维（空间）位置精度因子
• HDOP (Horizontal)：范围：0.5 - 99.9，包括经度和纬度等因子，称为水平（平面）位置精度因子
• VDOP (Vertical)：范围：0.5 - 99.9，仅包括高程因子，称为高程精度因子
• TDOP (Time): 钟差几何精度因子

精度衰减因子(DOP)是位置质量的指示器。它是考虑每颗卫星相对于星座（几何位置）中其它卫星的位置来预计用该星座能得到的位置精度的计算结果。小的DOP值表示强的卫星几何位置和精度的较高概率。高的DOP值表示弱的卫星几何位置和精度的较低概率.

高度误差按理应该采用VDOP，但是BetaFlight代码中使用的是HDOP，与理论有些不符。为什么？？？

2.3 数值分析

这里我们结合一组DEBUG的数据，分析了BetaFlight 4.3.1固件对高度的计算，实际一直在飞，所以理论上不可能出现高度为负值的情况。

表格中数据显示：

1. GPS高度被采用时刻==》正值
2. GPS高度后面随着时间/高度变化==》出现负值
3. Baro高度起飞瞬间==》负值
4. Baro其他飞行时间==》正值

因此，相对来说Baro气压计的高度更加精准。

DEBUG数据	起飞校准	起飞瞬间	GPS有效时刻前	GPS有效时刻后	HDOP1变化前	HDOP1变化后	HDOP2变化前	HDOP2变化后
GPS trust *100	0	0	0	43	43	28	28	43
Baro Altitude	0	-385	341	336	229	225	104	100
GPS Altitude	8896	8746	10014	340	40	39	-110	-146

3. 总结

根据上面视频以及数据的情况来看:BetaFlight的高度计算无法很好的支撑Auto Landing功能，真的基于气压计+GPS高度和这种融合算法来做Auto Landing一定是缺乏安全的软着陆（比如：接近地面时降低下降率，检测碰撞来判断是否着陆）。

同时，我们也可以看到第二个视频，除了起飞瞬间气垫效应影响了高度，出现负值，，全程整体效果还是不错的。也许该天气下VDOP的精度比较高，但是这个对GPS卫星依赖的情况无法很好支持实际应用Auto Landing功能。

PS：通过iNav的代码，我们可以比较好的发现，其高度计算已经与cleanFlight最初的代码逻辑不一样，整体重构过了，还处理了接地或者低空接地飞行的气垫效应。