你的自定义unwrap函数存在明显逻辑错误，同时MATLAB自带函数的用法可能也没匹配数据特性，以下是针对性的解决方法：

修复自定义Unwrap函数的逻辑错误 #

你的代码里未初始化yaw_unwrapped变量，且错误地将当前数据与未赋值的yaw_unwrapped(i)比较——正确逻辑应该基于当前数据与前一个已解包值的差值，通过加减周期整数倍（角度是±360°，弧度是±2π）来保证数据连续。

角度单位的正确手动实现 #

function unwrapped = manual_unwrap_angle(data)  
    unwrapped = zeros(size(data));
    unwrapped(1) = data(1); % 初始化第一个值
    for i = 2:length(data)
        diff = data(i) - unwrapped(i-1);
        % 根据差值调整，让数据保持连续
        unwrapped(i) = data(i) - round(diff / 360) * 360;
    end
end

弧度单位的正确手动实现 #

把上述代码中的360替换为2*pi即可。

MATLAB自带unwrap的正确用法 #

MATLAB的unwrap默认处理弧度数据，需根据你的数据单位调整：

• 数据为角度：指定容差为180（对应角度的半周期）

  unwrapped_data = unwrap(data, 180);
  

• 数据为弧度：直接使用默认容差π即可

  unwrapped_data = unwrap(data);
  

处理非周期性异常跳变 #

如果原始数据存在传感器噪声、突变干扰这类非周期性跳变，单纯unwrap无法解决，需先做预处理：

• 用移动平均平滑数据：

  smoothed_data = smoothdata(data, 'movmean', 5); % 5点移动平均
  unwrapped_smoothed = unwrap(smoothed_data, 180); % 平滑后再解包
  

• 或用低通滤波器去除高频噪声，比如用fir1设计滤波器后处理。

验证步骤 #

1. 确认数据单位（角度/弧度），这是unwrap生效的核心前提；
2. 检测原始数据中的异常点：找出与相邻值差值远大于180°（或π弧度）的点，区分是正常周期跳变还是传感器异常；
3. 根据数据情况选择“先平滑再解包”或“先解包再平滑”的顺序。

内容的提问来源于stack exchange，提问作者Shika93