最近刚把一套基于 Artix7 的到达角测量板调通核心就是用最经典的相位干涉仪原理做测向。项目本身不算复杂但真正动手时会发现在 FPGA 上落地和书本上的公式推导完全是两码事尤其是 FFT IP 核的配置、多通道相位一致性、角度解算时的定点精度每一步都有坑。这篇我把从原理到上板的完整过程都整理出来重点聊聊 Xilinx FFT IP 核的配置陷阱和相位差提取链路给正在做测向、辐射源定位或者想用 FPGA 练手信号处理的朋友做个参考。做这个项目之前你最好先了解一点FPGA 擅长的是并行流水线处理相位干涉仪测向本质上就是“多通道同时采样、做 FFT、比相位、出角度”这条链路天然适合用 FPGA 来干。如果你只是想验证算法用 Python 或 Matlab 当然更快但真要上设备、做实时处理Artix7 这个级别的芯片足够应付而且成本可控。后面所有的实现细节都是我在实际工程里调过的照着做能省不少时间。1. 项目整体设计与测向原理拆解1.1 相位干涉仪到底在测什么先把这个原理用大白话说清楚。假设有两个天线一个在左边一个在右边两者之间的距离叫基线记作 d。一束电磁波从远方射过来因为波前是倾斜的所以它到达两个天线的路程不一样于是两个通道收到的信号之间就有了一个时间差反映在相位上就是一个相位差。这个相位差和来波方向之间是严格对应的。设信号波长为 λ来波方向与天线法线夹角为 θ那么路程差就是 d·sinθ相位差就是Δφ (2π · d · sinθ) / λ反解一下就是θ arcsin(λ · Δφ / (2π · d))所以只要测出两个通道之间的相位差就能算出波从哪里来。这个公式看起来简单但工程上有一个非常重要的限制当 d λ/2 的时候sinθ 的取值范围可能让 Δφ 超过 ±π于是出现相位模糊。后面我会专门讲怎么用长短基线组合来解模糊这里先记住这个基线约束条件。我选的信号频率是 2.4GHz波长约 125mm。为了先跑通链路基线 d 定在 50mm也就是 0.4 个波长。这个距离小于半波长理论上可以无模糊地覆盖 ±90° 范围适合做原理验证。如果后续要做更高精度的测向再把基线加长同时引入解模糊逻辑。1.2 为什么选 Artix7 而不是 DSP 或单片机可能有人会问测个相位差而已用 STM32 加一个双通道 ADC 不行吗或者用 DSP 芯片处理不更简单这个问题我在项目立项时也纠结过。如果用单片机软件做 FFT 和复数运算当然也行但瓶颈在采样同步。相位干涉仪对两个通道的采样时刻一致性要求非常高稍有偏差就会直接变成相位误差。单片机的多通道 ADC 通常是轮流采样通道间天然存在时间差加上中断响应抖动相位差能算出来但精度很难保证。用 DSP 方案精度会好一些但你也需要外挂高速双通道 ADC并且 DSP 的并行性不如 FPGA。当你要同时处理多路信号、做多基线解模糊、后续还要加脉冲测向或者宽带测向时DSP 的实时性就会吃紧。Artix7 的优势在于一是它有大量可用 IO可以接双通道并行 LVDS 采样真正做到同一时钟沿采集两路数据二是 FPGA 内部可以做真正的并行流水线FFT 处理单元一旦跑起来数据是源源不断流式处理的三是它有丰富的 DSP48 乘法器资源做复数乘法和幅度比较很方便。价格上 Artix7 也比 Kintex7、Virtex7 友好得多开发板几百块就能入手很适合做这类信号处理原型验证。我当时选的型号是 XC7A35T资源不算多但跑一个双通道 4096 点 FFT 加解算逻辑还是够的。如果你预算多一点直接用 XC7A100T 就更从容后文我会说资源方面的取舍。1.3 系统架构与指标规划整个系统的信号链路是两天线信号经过低噪声放大和滤波后进入双通道 ADC 同步采样采样数据进入 FPGA先做数字下变频DDC或者直接对中频信号做 FFT然后找到信号频点对应的 FFT bin提取两个通道在该 bin 上的复数谱线用共轭相乘得到相位差最后经过角度映射输出到达角。我实现的模块划分大致是ADC 接口模块负责接收 LVDS 数据完成 Bit Alignment 和 Channel Alignment输出两路并行的 12bit 采样流。FFT 处理模块使用 Xilinx FFT IP 核配置为双通道模式两个通道并行做 4096 点 FFT。峰值搜索模块在 FFT 输出中找幅度最大的 bin作为信号频点。相位差提取模块取两个通道在峰值 bin 上的复数值做共轭相乘再通过 CORDIC 计算相位角。角度解算模块把相位差映射为到达角并通过串口输出到上位机。系统指标上我定位的采样率是 50MHzFFT 点数为 4096频率分辨率为 50MHz / 4096 ≈ 12.2kHz。这个分辨率对窄带连续波信号来说足够用了。后续如果你要做宽带信号测向可能需要用信道化结构那就是另一个话题了。2. 硬件平台与 FFT IP 核配置避坑2.1 前端采样链路设计要点相位干涉仪的关键在于通道间相位一致性所以前端设计要格外注意。ADC 我选的是双通道同步采样的型号比如 AD9268 这类两个通道共用同一个采样时钟这是保证相位差准确的前提。如果硬件上两个通道的时钟路径长度不一致即使 ADC 本身同步也会引入固定的通道间相位差。这个偏差可以在后期校准时补偿但最好在 Layout 阶段就尽量让两条时钟和信号走线等长。数据进入 FPGA 之后LVDS 接口需要做串并转换。这里有个容易踩的坑LVDS 的 Bit Alignment 必须做训练。ADC 上电后会输出训练序列FPGA 侧需要不断调整 Bitslip直到对齐到正确的比特边界。如果这一步没做对采样数据就是错位的相位差自然也是错的。我第一次上板就遇到这个问题采集到的波形全是乱码折腾了半天才发现是 LVDS 对齐没完成。FPGA 内部的数据路径上两个通道要严格同步进入 FFT。最简单的方法是在 ADC 接口模块完成后用 FIFO 做一个跨时钟域缓冲确保两路数据在同一时刻被送入 FFT 的输入总线。这一层如果没做后面所有的相位计算都白搭。2.2 FFT IP 核参数逐项拆解Xilinx FFT IP 核是 Vivado 里最常用的信号处理 IP配置界面有很多选项每一项对最终结果都有影响。我按自己的实际配置逐项说一下顺便把容易踩的坑标出来。第一项是 Number of Channels。这里要选 2两个通道同时做 FFT。IP 核内部会自动交叉处理两路数据输出时通过通道 ID 字段区分。双通道模式的资源占用比单通道高但换来了严格的同步性做相位差测量必须这样。如果你贪省资源把两个通道分时复用同一个 FFT处理时会引入时间偏差对窄带信号可能影响不大但相位精度会打折扣。第二项是 Transform Length。我设的是 4096。点数越大频率分辨率越高但资源占用和计算延迟也越大。对 2.4GHz 连续波测向4096 点已经够用。如果你的信号是宽带或跳频的可能要考虑更小的点数和信道化结构。第三项是 Architecture Choice。这里有几个选项Pipelined Streaming、Radix-4 Burst I/O、Radix-2 Burst I/O、Radix-2 Lite Burst I/O。我推荐 Pipelined Streaming因为它可以连续处理数据流符合测向系统不断采样的需求。但要注意Pipelined Streaming 资源占用最高。如果资源紧张可以用 Radix-4 Burst I/O代价是处理完一帧数据后会有一段空窗期不能做到全流水。第四项是 Data Format。强烈建议选 Block Floating Point块浮点也就是 IP 核的 Floating Point 选项里的 Block Floating Point 模式。这里很多人被名字绕晕Block Floating Point 其实本质还是整数运算只是一整帧数据共用一个缩放因子能有效防止中间过程溢出。对于测相位差来说它特别友好因为我们最终比较的是同一频点上两路复数的比值公共缩放因子在除法中会被抵消你根本不需要关心绝对幅度是多少相位关系保持不变。如果用手动缩放Scaled就要自己算 SCALE_SCH 寄存器稍有不慎就会溢出或损失精度。我第一次就是用手动缩放算错了几次后来换成块浮点省心多了。第五项是 Output Order。选 Natural Order 比较方便FFT 输出的 bin 顺序就是 0 到 N-1峰值搜索逻辑不用做索引反转。如果选 Bit-Reversed Order也能处理但后续要自己重排白白增加逻辑复杂度。还有一个容易忽略的地方IP 核配置界面有个 Target Clock Frequency这个数值只是用来让 IP 核做时序评估的并不是说你的时钟必须精确等于这个值。实际使用时只要满足 IP 核生成报告里的时序要求就行。我们系统主时钟是 100MHz我在配置里写的也是 100MHz这样最稳妥。至于很多人问的“FFT IP 核不能设置小数时钟输入”其实是因为大家把采样时钟和 FFT 的工作时钟混为一谈了。FFT IP 核本身只有一个 aclk 输入它接收的是 AXI4-Stream 数据只要 tvalid 有效就采一个数。如果你想实现 40.5MHz 的采样率完全可以让 aclk 跑在 81MHz然后用控制逻辑每隔一个周期拉高一次 tvalid等效采样率就是 40.5MHz。IP 核不需要知道“采样率”这个概念它只关心每个时钟沿是不是有有效数据。所以不是不能设小数而是要用 tvalid 心跳来控制。2.3 帧同步与数据输出时序FFT IP 核的 AXI4-Stream 接口有几个信号必须理解清楚否则写状态机时很容易出错。s_axis_data_tvalid 表示输入数据有效。s_axis_data_tlast 表示一帧的最后一个数据。配置好 FFT 核之后IP 核内部会自动管理帧的切分你只需在输入侧每 N 个有效数据拉一次 tlast。m_axis_data_tvalid 是输出有效信号m_axis_data_tlast 是输出一帧结束标志。输出侧的 tdata 位段要特别注意按 Xilinx 的文档对于定点或块浮点模式TDATA 的低 16 位是虚部高 16 位是实部。我第一次就按直觉以为低 16 位是实部结果相位符号全反了排查了很久。写代码之前一定要仔细看 PG109 里的位段定义。输出侧还有一个 xk_index 字段通常映射到 TUSER 的低位它指示当前输出对应的是第几个频点。在自然序模式下它从 0 递增到 N-1。峰值搜索模块就是靠它来记录最大幅度对应的 bin 位置。另外多通道模式下TUSER 里还有通道 ID 字段用来区分当前输出属于第几个通道。取相位差时必须根据通道 ID 分别缓存两个通道的峰值复数值不能混在一起。3. 相位差提取与角度解算3.1 峰值搜索与频率对齐FFT 做完之后第一步是找到信号落在哪个 bin 上。对于连续波窄带信号最简单的办法就是遍历所有输出 bin找出幅度平方最大的那一个。幅度平方的计算方式是mag_sq real^2 imag^2这里我直接用 DSP48 乘法器算不推荐用 abs(real) abs(imag) 的近似方式因为近似方式在峰值检测时容易出现误判尤其在信噪比不高的情况下。Artix7 的 DSP48 数量不少4096 点 FFT 本身已经用了不少再多算两个乘法是没问题的。峰值搜索状态机的思路是当 tvalid 有效时计算当前 bin 的幅度平方。如果大于之前记录的峰值则更新峰值寄存器和峰值索引。当 tlast 到来时把峰值索引锁存然后清空峰值寄存器为下一帧做准备。这里有个时序细节FFT 输出的最后一个 bin 的 tvalid 拉高后紧接着 tlast 才会拉高。峰值搜索逻辑必须在 tlast 之前完成最后一轮比较否则最后一两个点的峰值可能丢失。稳妥的做法是在 tvalid tready 的时钟沿做比较tlast 到来时直接把缓存的最大值作为本帧结果。写代码时要注意比较器的复位时机别把当前帧的数据带进下一帧。3.2 跨通道相位差提取找到峰值 bin 之后把两个通道在该 bin 上的复数谱线取出来。假设通道 1 的谱线是 X1 a1 j·b1通道 2 的谱线是 X2 a2 j·b2。相位差可以通过共轭相乘得到Z X1 · conj(X2) (a1·a2 b1·b2) j·(b1·a2 - a1·b2)相位差就是Δφ atan2(Im(Z), Re(Z))这个复数乘法在 FPGA 里用几个乘法器和加法器就能实现。需要注意的是定点数位宽FFT 输出的数据位宽如果不够乘法结果的位宽会快速增长。我建议把两个复数的实部和虚部都扩展到 18bit 再做乘法乘积结果保留 32bit。位宽太窄会导致相位噪声变大位宽太宽则浪费资源。计算 atan2 我直接用 Xilinx 的 CORDIC IP 核配置成 Vector Translational 模式输入是 Re(Z) 和 Im(Z)输出是角度相位范围在 -π 到 π 之间。CORDIC 的输出位宽决定角度分辨率我选了 20bit 的相位精度对应约 0.00006 弧度的分辨率已经远高于系统需要的精度。CORDIC 核的延迟是固定的十几个时钟周期正好适合流水线处理。这里要提醒一点相位差的原始结果始终在 -π 到 π 之间。如果真实的相位差在时间上是连续变化的比如目标在移动那么相位差可能跨过 ±π 的边界导致输出角度发生跳变。这种情况需要做相位解缠绕Phase Unwrapping比较当前帧和上一帧的相位差如果跳变超过 π就加上或减去 2π 的整数倍让相位轨迹保持连续。3.3 角度解算与相位解模糊相位差测出来之后就可以套用干涉仪公式算角度了θ arcsin(λ · Δφ / (2π · d))这个反正弦函数在 FPGA 里实现有几个选择。最简单的是用查找表把 Δφ 的取值范围均匀量化后查表得到 θ。缺点是精度受表深限制而且如果 Δφ 范围很大表会很大。我使用的是 CORDIC IP 核的 Arcsin 模式或者更通用的做法是先用 atan2 得到相位差再用一个多项式近似计算 arcsin。考虑到测试信号比较稳定我直接例化了第二个 CORDIC 核做反正弦代码简单效果也很理想。实际计算时要注意输入坐标系的问题。两个天线组成的基线只能测出一个“圆锥角”也就是来波方向与基线法线的夹角单基线是无法区分方位角和俯仰角的。真要定位目标需要第二组正交基线。我当前项目只做单基线到达角验证所以输出的是目标相对天线法线的夹角也就是一个一维角度。如果基线大于半波长相位差会出现模糊这时需要解模糊。工程上常用的方法是长短基线组合短基线 d_short λ/2保证没有模糊但精度有限长基线 d_long 精度高但模糊。先用短基线解出一个粗略角度 θ_coarse再用长基线测量出的相位差 Δφ_long 解模糊k round((d_long/d_short · Δφ_short - Δφ_long) / (2π))Δφ_unamb Δφ_long 2πk然后用这个解模糊之后的相位差重新计算精确角度。这个算法在 FPGA 里做也不复杂主要就是一个除法加一个取整。后续如果我要扩展成长基线就直接按这个逻辑加进去。3.4 定点数与精度控制细节整条处理链路上我最开始以为只要 FFT 输出正确后面就顺理成章但实际上定点数精度处理才是影响最终角度稳定性的关键。ADC 输出是 12bit 有符号数FFT 输入是 16bit。我把 ADC 数据左移 4 位把信号幅度尽量顶到满幅附近。这样做的好处是最大化信号量化信噪比避免小信号在 FFT 后被淹没在量化噪声里。但要小心如果输入信号本身已经接近满幅左移 4 位会溢出。所以输入前必须加一个幅度检测和自动增益控制或者至少留出 3-4 个比特的余量。我在测试时是手动调整信号源输出功率保证 ADC 输入峰值在 60% 到 80% 满幅之间这样既不溢出信噪比也够。FFT 的块浮点模式会自动调整缩放因子这一层你不用管。但要记住块浮点的输出没有统一的定标不同帧之间的缩放因子可能不同。不过正如前面说的因为我们只关心两个通道在同一帧内的相位关系公共缩放因子在共轭相乘和反正切运算中会被抵消所以这个特性对我们来说完全不是问题。共轭相乘后的复数值在送入 CORDIC 之前我做了归一化处理。具体做法是把 Re(Z) 和 Im(Z) 都限制在 CORDIC 核的输入位宽范围内如果超出范围就右移若干位。这样做的目的是防止 CORDIC 内部的定点运算溢出。有个小技巧可以开一个额外的标志记录右移位数但反正切函数对输入进行等比例缩放时不改变角度所以这个标志其实不需要传给后续模块只用保证输入值在合法范围内就行。4. 工程集成、仿真与上板调试4.1 模块划分与数据流设计整个工程的模块化程度直接决定调试效率。我按数据流把一个顶层设计拆成了五个子模块每个子模块的输入输出都用 AXI4-Stream 或者简单的 valid/ready 握手这样任何一个环节出问题都可以单独用 ILA 查看。数据流大致是ADC 接口模块输出两路并行 12bit 数据 → 送入同步模块打拍对齐后扩展为 16bit 并分别送入 FFT IP 核的通道 0 和通道 1 → FFT IP 核输出两路频谱 → 峰值搜索模块找到当前帧的峰值 bin → 相位差模块根据峰值索引取出两个通道的复数谱线并做共轭相乘 → CORDIC 算相位差 → 角度解算模块映射成角度值 → 串口发送模块把角度和峰值索引打包输出。模块之间的接口我统一用 32bit 数据总线尽量少用手写握手机制避免模块间出现时序竞争。子模块内部可以根据需要自由使用 valid/ready但跨模块的地方能简单就简单。4.2 仿真验证流程我习惯在写 RTL 之前先用 Matlab 生成一组测试向量生成两个同频正弦信号设定其中一个比另一个超前一定相位量化成 12bit 后保存成 COE 文件或 textbench 直接读入。这样在仿真时就能精确知道理论角度和 RTL 输出对比快速定位问题。Verilog testbench 的主要逻辑是根据采样率生成时钟。读取两个通道的测试数据按帧送入设计。等待一段时间后抓取角度输出。与理论值比较打印误差。第一次仿真时我就发现角度输出一直不对检查后发现是 FFT 输入数据的符号位处理错误ADC 数据是二进制补码但我在拼接 16bit 时直接高位补了 0把小信号变成了带直流偏置的信号。这让我意识到FPGA 里做有符号数扩展必须用符号位扩展或者明确用 $signed() 做转换。仿真通过后再上板调试就轻松很多。但仿真毕竟是理想环境很多硬件问题比如 LVDS 对齐、时钟抖动仿真里看不出来。所以我的建议是尽量把仿真做细把算法问题都在仿真阶段解决上了板只处理硬件相关问题。4.3 上板调试流程上板调试我分为三步走。第一步先不接 FFT直接用 ILA 抓 ADC 输出确认两路采样数据波形正确、幅度正常、无毛刺。这一步能快速排除 LVDS 对齐和跨时钟域问题。如果两路波形在 ILA 里看起来完全同相说明前端链路没问题。第二步把 FFT 接入仍用连续波信号源输入用 ILA 抓 FFT 输出的幅度谱。主要看峰值是否在预期的 bin 位置以及两个通道的峰值幅度是否接近。如果通道 1 峰值很大通道 2 峰值很小说明通道间幅度一致性有问题可能是前端增益不匹配或者 LVDS 位对齐有误。第三步把角度解算链路全部打开观察输出的角度值。此时如果信号源输出同相信号角度理论上应该是 0°实测如果有一个固定偏角说明通道间存在固定相位差需要做校准。我在测试时发现系统有大约 2.3° 的固定偏差追查原因是 ADC 两个通道的模拟前端滤波器延迟不完全一致。于是我在 FPGA 内加了一个校准常数把这 2.3° 在角度解算时减去。这一步做完角度输出就非常干净了。5. 常见问题与排查技巧实录5.1 高频故障速查表我把项目过程中遇到的高频问题整理成了一个速查表方便你调试时直接对号入座。问题现象可能原因解决方法角度输出乱跳无规律相位差跨越 ±π 边界增加相位解缠绕处理角度输出始终偏差一个固定值两通道前端模拟链路相位不一致注入同源信号校准减掉常数偏差FFT 峰值找错 bin输出顺序不是自然序检查 IP 配置设置为 Natural Order角度输出噪声大、抖动明显输入信号幅度太低或量化位宽不足保证 ADC 输入接近满幅检查是否有 DDC 增益不足tvalid 经常拉低数据丢帧FFT 处理速度跟不上输入速率降低 FFT 点数或提高 aclk 频率复位后第一次输出异常之后正常FPGA 复位信号亚稳态或复位释放不同步使用异步复位同步释放电路双通道输出串扰FFT 多通道模式下通道 ID 没有区分用 TUSER 中的通道 ID 字段分别锁存FFT 输出复数实部虚部搞反对 TData 位段定义理解错误核对 PG109低 16 位是虚部高 16 位是实部5.2 典型排障案例复盘第一个想复盘的问题是“角度恒定偏大一度”。现象是角度输出稳定但和理论值总是差一个固定的量。一开始我怀疑是 FFT 相位计算问题折腾了好久也没找到原因。后来用信号源分别输入两个通道同一个信号结果角度输出是 1° 而不是 0°才意识到是硬件通道间的固有相位差。解决办法很简单在信号链路里强行让两个通道短接测量这个固定偏差然后在角度解算前把它减掉。这个问题提醒我FPGA 里的算法再准也架不住前端模拟链路的不一致性校准是必经步骤。第二个问题是“FFT 输出峰值对不上”。我把一个 5MHz 的单音信号送入系统理论上应该落在第 410 个 bin 附近5MHz / 12.2kHz ≈ 410但实际峰值出现在第 405 个 bin。排查后发现问题出在 FFT 的输出顺序上我配置时选了 Bit Reversed Order但峰值搜索模块是按自然序做的导致索引错位。改成 Natural Order 后问题就消失了。这个问题的教训是配置 IP 时一定要把输出顺序和后续逻辑对齐不要想当然。第三个问题是“系统工作一段时间后角度突然变成固定值不再变化”。这个现象很诡异最后通过 ILA 抓内部信号发现是 FFT 输入侧的一帧数据偶尔多了一个数导致帧边界错位。原因是我在 ADC 接口模块里用 FIFO 做跨时钟域缓冲时写入侧的帧结束标志和读出的帧结束标志没有严格对齐偶尔会差一拍。解决办法是用 tlast 来同步 FIFO 读写指针的帧边界每帧的第一个数据强制从 FIFO 空的时刻开始计数。这个问题让我意识到帧同步在流式处理里是一个核心问题不能只靠 valid 信号。5.3 几条少有人提的避坑技巧第一FFT 输入数据的符号扩展千万别想当然。ADC 输出 12bit 补码扩展成 16bit 时一定要按符号扩展。直接用零填充会引入一个直流偏置导致频谱 0 频附近出现假峰严重时会把真正的信号峰值淹没。实际调试时这个问题在仿真阶段就能发现所以强烈建议第一次跑通代码前用 Matlab 生成半周期正弦波做测试向量一眼就能看出问题。第二单通道测角时如果目标是移动的相位差输出大概率会出现 ±π 的跳变。这时候在角度输出端直接做平滑滤波是没有用的必须先做相位解缠绕让相位差先变成连续曲线再滤波。我最终在相位差模块后加了简单的滑动平均效果比直接滤波好得多。第三如果你想省一块 FFT IP 核可以考虑把两路 ADC 数据合路成一个复数输入一路放实部一路放虚部做复数 FFT然后利用 FFT 的共轭对称性分离出两路频谱。这个技巧在某些资源紧张的场合很有用但实现复杂度高新手不建议上来就这么搞。老老实实用双通道模式最稳妥。6. 最后分享一点我的实操体会6.1 我习惯性的几个工程细节每次做 FPGA 信号处理项目我都会先花半小时把数据格式和位宽定义清楚再开始写 RTL。这个项目里最让我头疼的其实不是算法而是各种位宽不对、符号不对、帧不同步。现在我的习惯是在每个模块的接口注释里直接写明数据格式比如 “s[15:0] Q1.15, real part”防止自己过两周忘了当初的设计意图。还有一点Artix7 的资源比想象中要紧张。XC7A35T 做双通道 4096 点 Pipelined Streaming FFT 时LUT 利用率大概到 80% 左右布线压力很大。如果后期还要加卡尔曼滤波或更多逻辑建议直接上 XC7A100T省下来的时间比省下的芯片钱值钱得多。6.2 如果让我重新做一遍我会怎么改如果再做一个版本我会在前端加一个自动增益控制让 ADC 输入幅度在不同距离下都能保持在接近满幅的状态。这样 FFT 输出的信噪比更稳定角度解算的抖动也会小很多。另外当前实现里角度输出是逐帧直接吐出来的没有做后处理。实际测试中静止目标的单帧角度波动大概在 ±0.5° 左右对于演示足够但要做装备级应用远远不够。我会在输出端加一个卡尔曼滤波器或者指数滑动平均把角度平滑到 ±0.1° 以内。这个改动逻辑上不算难关键是别和相位解缠绕打架一定要先解缠绕再滤波。反过来说如果当时一上来就做滤波估计又得踩不少坑。调试这类系统一步一步来比什么都重要。