用于多读数头旋转编码器的三角卡尔曼滤波融合算法（1）

一、传统方法与TKF的比较% 传统方法先计算角度再滤波 % 步骤1从 sin/cos 计算角度 theta_raw atan2(sin_signal, cos_signal); % 步骤2对角度进行滤波 theta_filtered kalman_filter(theta_raw); % 在步骤1中噪声和非线性被引入了% TKF方法直接使用 sin/cos 信号 % 步骤1sin/cos 信号直接作为测量值 z [sin_signal; cos_signal]; % 步骤2在三角函数空间进行滤波 % 步骤3最后才计算角度 theta_estimated atan2(x_hat(1), x_hat(2)); % 避免了 atan2 计算引入的非线性和噪声放大二、验证atan2计算引入的非线性和噪声放大% 验证 atan2 对噪声的放大效应为达到效果添加的噪声会较大。 % 生成干净的 sin/cos 信号 theta_true linspace(0, 2*pi, 1000); sin_true sin(theta_true); cos_true cos(theta_true); % 添加小噪声 noise_std 0.01; sin_noisy sin_true noise_std * randn(size(theta_true)); cos_noisy cos_true noise_std * randn(size(theta_true)); % 方法1直接计算角度 theta_direct atan2(sin_noisy, cos_noisy); theta_direct_unwrap unwrap(theta_direct); % 方法2先滤波 sin/cos再计算角度 window_size 10; sin_filtered movmean(sin_noisy, window_size); cos_filtered movmean(cos_noisy, window_size); theta_filtered atan2(sin_filtered, cos_filtered); theta_filtered_unwrap unwrap(theta_filtered); % 计算误差 error_direct rad2deg(wrapToPi(theta_direct_unwrap - theta_true)); error_filtered rad2deg(wrapToPi(theta_filtered_unwrap - theta_true)); % 绘图 figure(1); subplot(1,2,1); plot(theta_true, sin_true, b); hold on; plot(theta_true, sin_noisy, r);hold on; plot(theta_true, cos_true, g);hold on; plot(theta_true, cos_noisy, y);hold on; xlabel(真实角度 (rad)); ylabel(sincos值); title(sincos信号); legend(sin不含噪声, sin含噪测量,cos不含噪声, cos含噪测量); grid on; subplot(1,2,2); plot(theta_true, error_direct, r-, LineWidth, 1); hold on; plot(theta_true, error_filtered, b-, LineWidth, 1); xlabel(真实角度 (rad)); ylabel(角度误差 (度)); title(atan2 对噪声的放大效应); legend(直接计算角度, 先滤波sin/cos再计算角度); grid on;% 验证 atan2 在 sin/cos 接近0时的数值不稳定性 % 在 θ ≈ 0 附近 theta 0.01; % 小角度 sin_val sin(theta); % ≈ 0.01 cos_val cos(theta); % ≈ 0.99995 % 添加噪声 noise_levels [0.001, 0.005, 0.01, 0.05, 0.1]; fprintf(角度 θ %.4f rad (%.2f°)\n, theta, rad2deg(theta)); fprintf(sinθ %.6f, cosθ %.6f\n\n, sin_val, cos_val); fprintf(噪声标准差\tatan2误差\t误差放大倍数\n); fprintf(----------------------------------------\n); for i 1:length(noise_levels) noise_std noise_levels(i); errors zeros(1000, 1); for j 1:1000 sin_noisy sin_val noise_std * randn(); cos_noisy cos_val noise_std * randn(); theta_est atan2(sin_noisy, cos_noisy); errors(j) abs(theta_est - theta); end mean_error mean(errors); amplification mean_error / noise_std; fprintf(%.4f\t\t%.6f rad\t%.2f\n, noise_std, mean_error, amplification); end三、三角卡尔曼滤波的核心1、转换在三角函数进行转换而非角度进行转换避免非线性转换引入的误差2、数据融合建立统一模型同时融合所有组sincos信号3、谐波建模在状态向量中引入谐波系数实现对测量误差的在线补偿。四、三角卡尔曼滤波算法流程未完待续明日更新。