企业官网建设流程全解析

一、卡尔曼滤波算法

1.1 一维卡尔曼滤波算法

卡尔曼滤波算法是一种最优线性状态估计算法，是一种离散的算法，分为预测与观测两部分。

在运行滤波器之前，需要设置观测噪声和过程噪声，这决定了滤波器更相信观测值或更相信预测值。

首先通过被控对象的数学特性、当前系统输入以及之前得到的数据预测现在的数据。然后通过获取被控对象数据的观测器观测现在的数据，更新得到计算卡尔曼增益。最后滤波得到的数值即为，相当于预测值与观测值的加权平均数。

公式如下：

首先初始化观测噪声、过程噪声、预测可信度、滤波器初始值。

在第1个时间步长后，获取此时状态量观测值和输入量观测值，进行第1次卡尔曼滤波：

....................................

....................................

在第k个时间步长后，获取此时状态量观测值和输入量观测值，进行第k次卡尔曼滤波：

1.2 拓展卡尔曼滤波算法

拓展卡尔曼滤波算法将维度拓展至多维，此时在预测、更新时需要考虑矩阵中存在对角线元素以外的元素。协方差便用于量化估计的不确定性。对于一个向量，计算得到其均值为，协方差矩阵定义为：（只是理论，是一种线性运算方式，根据具体情况具体调整）

需要注意，当存在线性变换时，有：

在引入协方差概念后，1.1中部分变量定义为：观测噪声协方差矩阵、过程噪声协方差矩阵、预测值协方差矩阵、状态转移矩阵。

在卡尔曼滤波算法中，存在通过上一步数值预测当前数值和通过测量数值更新滤波数值两部分。在预测部分中，算法使用类似的方式进行预测，因此在预测过程中，对当前的预测值协方差矩阵的计算过程中需要采用类似的方式计算。

公式如下：

对于更新卡尔曼滤波器的算法，需要引入概念新息。新息用于表示预测值与测量值之差的协方差，设观测矩阵为，新息为，即：

则新息的协方差矩阵为：

注意到用于新息协方差矩阵运算的矩阵是。

公式如下：

二、用于四元数的拓展卡尔曼滤波算法

已经得到四元数在连续时间下的状态转移矩阵：

需要将上式转化为适用于卡尔曼滤波器的离散时间的状态转移矩阵。

假设，则有。求解该微分方程，解为。

对进行泰勒展开：

保留两位，即为：

对于卡尔曼滤波器，需要通过时刻的得到时刻的，则将上式转化为：

式中，为滤波器工作周期。

三、MATLAB仿真

3.1 飞行器真实姿态设置

设置飞行器2s的飞行姿态，为俯仰角按三角函数变化一个周期（从0°到10°到0°），模拟抬头后恢复的过程：

%% 1. 时间设置与真实姿态（ZYX 欧拉角顺序） t = 0:0.01:2; % 时间向量，0～2秒，步长0.01s N = length(t); % 总采样点数 T = 0.01; % 采样周期 phi_true = zeros(1, N); % 真实滚转角（roll），单位：弧度 theta_true = deg2rad(10 * (1 - cos(pi * t))); % 真实俯仰角（pitch），动态变化 psi_true = zeros(1, N); % 真实偏航角（yaw），保持为0 % 计算欧拉角的时间导数（用于生成真实角速度） dphi_dt = zeros(1, N); dtheta_dt = deg2rad(10 * pi * sin(pi * t)); % 对 theta_true 求导 dpsi_dt = zeros(1, N);

通过飞行器姿态求出飞行器角速度：

%% 2. 根据真实姿态计算机体真实角速度 [p; q; r]（FRD 体轴系） gyro_true = zeros(3, N); % 真实角速度矩阵（3×N） for k = 1:N phi = phi_true(k); theta = theta_true(k); % 构建从欧拉角速率到机体角速度的转换矩阵 T(phi, theta)（ZYX顺序） T_mat = [ ... 1, 0, -sin(theta); ... 0, cos(phi), sin(phi)*cos(theta); ... 0, -sin(phi), cos(phi)*cos(theta) ... ]; euler_rates = [dphi_dt(k); dtheta_dt(k); dpsi_dt(k)]; % 欧拉角速率向量 gyro_true(:,k) = T_mat * euler_rates; % 转换为机体角速度 end

3.2 模拟传感器

使用陀螺仪、加速度计、磁力计获取飞行器真实姿态，需要设置传感器噪声：

%% 3. 模拟 IMU 传感器测量值（带高斯噪声） rng(1); % 设置随机种子，保证结果可复现 gyro_noise_std = deg2rad(0.5); % 陀螺仪噪声标准差（0.5 度/秒） acc_noise_std = 0.05; % 加速度计噪声标准差（m/s²） mag_noise_std = 0.02; % 磁力计噪声标准差（归一化后） gravity = 9.8; % 重力加速度大小（m/s²） mag_earth = [1; 0; 0]; % 地磁场方向（北向，在 NED 坐标系中） % 初始化测量矩阵 gyro_meas = zeros(3, N); % 陀螺仪测量值 acc_meas = zeros(3, N); % 加速度计测量值 mag_meas = zeros(3, N); % 磁力计测量值 for k = 1:N % 当前真实姿态角 phi = phi_true(k); theta = theta_true(k); psi = psi_true(k); % 计算旋转矩阵 R_be：从 NED 地理坐标系 到 FRD 机体坐标系（ZYX 欧拉角） RX = [ 1 0 0 ; 0 cos(phi) sin(phi) ; 0 -sin(phi) cos(phi) ;]; RY = [ cos(theta) 0 -sin(theta); 0 1 0 ; sin(theta) 0 cos(theta);]; RZ = [ cos(psi) sin(psi) 0 ; -sin(psi) cos(psi) 0 ; 0 0 1 ;]; R_be = RX * RY * RZ; % 陀螺仪测量 = 真实角速度 + 高斯噪声 gyro_meas(:,k) = gyro_true(:,k) + gyro_noise_std * randn(3,1); % ✅ 加速度计测量模型修正： % 在 FRD 坐标系中，z 轴向下与重力同向 % 静止时，加速度计应输出 [0; 0; +g] % 因此：acc = R_be * g_ned，其中 g_ned = [0; 0; g] g_ned = [0; 0; gravity]; acc_body = R_be * g_ned; % 重力在机体坐标系中的投影 acc_meas(:,k) = acc_body + acc_noise_std * randn(3,1); % 磁力计测量 = 地磁场在机体坐标系中的投影 + 噪声（并归一化） mag_body = R_be * mag_earth; mag_body = mag_body / norm(mag_body); % 归一化地磁场 mag_meas(:,k) = mag_body + mag_noise_std * randn(3,1); mag_meas(:,k) = mag_meas(:,k) / norm(mag_meas(:,k)); % 再次归一化 end

得到不采用滤波器时传感器得到的数据：

%% 4. 直接由加速度计和磁力计解算姿态（无滤波） phi_direct = zeros(1, N); % 直接解算的滚转角 theta_direct = zeros(1, N); % 直接解算的俯仰角 psi_direct = zeros(1, N); % 直接解算的偏航角 for k = 1:N acc = acc_meas(:, k); mag = mag_meas(:, k); % ✅ 修正：加速度计直接给出重力方向（z 向下为正） % 因此重力单位向量 = acc / norm(acc) g_body = acc / norm(acc); % 根据重力向量解算 roll 和 pitch（FRD 坐标系） % 推导关系：g_body = [-sinθ, sinφ·cosθ, cosφ·cosθ]^T phi_direct(k) = atan2(g_body(2), g_body(3)); % 滚转角 φ = atan2(gy, gz) theta_direct(k) = -asin(max(-1, min(1, g_body(1)))); % 俯仰角 θ = -asin(gx)，注意负号 % 使用磁力计解算偏航角（需进行倾角补偿） phi = phi_direct(k); theta = theta_direct(k); cphi = cos(phi); sphi = sin(phi); cth = cos(theta); sth = sin(theta); % 将磁力计数据投影到水平面 mag_hx = mag(1)*cth + mag(2)*sth*sphi + mag(3)*sth*cphi; % 北向分量 mag_hy = mag(2)*cphi - mag(3)*sphi; % 东向分量 psi_direct(k) = atan2(-mag_hy, mag_hx); % 偏航角 ψ（注意符号） end

3.3 EKF算法

初始化EKF：

%% 5. EKF 初始化 x_hat = [1; 0; 0; 0]; % 初始四元数 [w; x; y; z] P = diag([0.01, 0.01, 0.01, 0.01]); % 初始协方差矩阵 Q = (T^2) * (gyro_noise_std^2) * eye(4); % 过程噪声协方差 R = diag([0.01, 0.01, 0.01, 0.01]); % 观测噪声协方差 q_est = zeros(4, N); % 存储 EKF 估计的四元数 q_est(:,1) = x_hat; % 初始值

EKF滤波：

%% 6. EKF 主循环（融合陀螺仪、加速度计、磁力计） for k = 2:N % 获取上一时刻的陀螺仪测量值（用于状态预测） p = gyro_meas(1, k-1); q_val = gyro_meas(2, k-1); r = gyro_meas(3, k-1); % 构建状态转移矩阵（基于四元数微分方程） F = [1, -0.5*T*p, -0.5*T*q_val, -0.5*T*r; 0.5*T*p, 1, 0.5*T*r, -0.5*T*q_val; 0.5*T*q_val, -0.5*T*r, 1, 0.5*T*p; 0.5*T*r, 0.5*T*q_val, -0.5*T*p, 1]; % 预测步骤 x_pred = F * x_hat; x_pred = x_pred / norm(x_pred); % 单位化四元数 P_pred = F * P * F' + Q; % 预测协方差 % 观测更新：由当前 acc 和 mag 解算观测四元数 [q0_obs, q1_obs, q2_obs, q3_obs] = get_Qua_FRD_Corrected(mag_meas(:,k), acc_meas(:,k)); z_obs = [q0_obs; q1_obs; q2_obs; q3_obs]; z_obs = z_obs / norm(z_obs); % 单位化 % 观测矩阵（线性化，此处简化为单位阵） H = eye(4); S = H * P_pred * H' + R; % 新息协方差 K = P_pred * H' / S; % 卡尔曼增益 % 处理四元数符号歧义（确保内积为正） y = z_obs - x_pred; if dot(x_pred, z_obs) < 0 y = -z_obs - x_pred; end % 更新步骤 x_hat = x_pred + K * y; x_hat = x_hat / norm(x_hat); % 单位化 P = (eye(4) - K*H) * P_pred; % 更新协方差 % 保存结果 q_est(:,k) = x_hat; end %% ============================== % 子函数：根据加速度计和磁力计数据计算四元数（修正版，适用于 FRD 坐标系） % 输入： % mag: 磁力计测量值（3×1 向量，已归一化） % acc: 加速度计测量值（3×1 向量，单位 m/s²） % 输出： % q0,q1,q2,q3: 四元数 [w,x,y,z] function [q0, q1, q2, q3] = get_Qua_FRD_Corrected(mag, acc) acc = acc(:); mag = mag(:); % ✅ 关键修正：加速度计直接反映重力方向（z 向下为正） % 因此重力单位向量 = acc / norm(acc) g_body = acc / norm(acc); % 从重力向量解算 roll 和 pitch phi = atan2(g_body(2), g_body(3)); % 滚转角 theta = -asin(max(-1, min(1, g_body(1)))); % 俯仰角（注意负号） % 倾角补偿后的磁力计解算 yaw cphi = cos(phi); sphi = sin(phi); cth = cos(theta); sth = sin(theta); mag_hx = mag(1)*cth + mag(2)*sth*sphi + mag(3)*sth*cphi; % 北向分量 mag_hy = mag(2)*cphi - mag(3)*sphi; % 东向分量 psi = atan2(-mag_hy, mag_hx); % 偏航角 % 将 ZYX 欧拉角转换为四元数 cr = cos(phi/2); sr = sin(phi/2); cp = cos(theta/2); sp = sin(theta/2); cy = cos(psi/2); sy = sin(psi/2); q0 = cr*cp*cy + sr*sp*sy; q1 = sr*cp*cy - cr*sp*sy; q2 = cr*sp*cy + sr*cp*sy; q3 = cr*cp*sy - sr*sp*cy; end

四元数转化欧拉角：

%% 7. 将 EKF 估计的四元数转换为欧拉角（ZYX 顺序） phi_est = zeros(1, N); theta_est = zeros(1, N); psi_est = zeros(1, N); for i = 1:N q0 = q_est(1,i); q1 = q_est(2,i); q2 = q_est(3,i); q3 = q_est(4,i); % 四元数转 ZYX 欧拉角（标准公式） phi_est(i) = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1^2 + q2^2)); theta_est(i) = asin(max(-1, min(1, 2*(q0*q2 - q3*q1)))); % 防止数值溢出 psi_est(i) = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2^2 + q3^2)); end

3.4 显示曲线

将真实姿态、未滤波传感器信息、滤波后传感器信息绘图：

%% 8. 绘图：真实值 vs 直接测量 vs EKF 滤波 figure('Position', [100, 100, 900, 700]); subplot(3,1,1); plot(t, rad2deg(phi_true), 'r', 'LineWidth', 2); hold on; plot(t, rad2deg(phi_direct), 'g--', 'LineWidth', 1.5); plot(t, rad2deg(phi_est), 'b:', 'LineWidth', 1.5); ylabel('滚转角 \phi (度)'); grid on; legend('真实值', '直接测量（加速度计）', 'EKF 滤波', 'Location', 'best'); subplot(3,1,2); plot(t, rad2deg(theta_true), 'r', 'LineWidth', 2); hold on; plot(t, rad2deg(theta_direct), 'g--', 'LineWidth', 1.5); plot(t, rad2deg(theta_est), 'b:', 'LineWidth', 1.5); ylabel('俯仰角 \theta (度)'); grid on; legend('真实值', '直接测量（加速度计）', 'EKF 滤波', 'Location', 'best'); subplot(3,1,3); plot(t, rad2deg(psi_true), 'r', 'LineWidth', 2); hold on; plot(t, rad2deg(psi_direct), 'g--', 'LineWidth', 1.5); plot(t, rad2deg(psi_est), 'b:', 'LineWidth', 1.5); xlabel('时间 (秒)'); ylabel('偏航角 \psi (度)'); grid on; legend('真实值', '直接测量（磁力计）', 'EKF 滤波', 'Location', 'best'); sgtitle('姿态估计对比（FRD 坐标系）：z 轴与重力同向 → 加速度计 z 分量为正');

结果如下：