企业官网建设流程全解析

简介：这是基于扩展卡尔曼滤波(EKF)的航天器姿态轨道耦合控制系统Matlab实现，面向航天器动力学与控制方向的高年级本科生、研究生及科研工作者，适用于课程设计、专题研究与毕业设计，采用模块化参数化编程，覆盖姿态确定、轨道动力学、EKF状态估计、PD/LQR控制等核心环节，并适配轨道维持、交会对接等任务场景，支持在2014a、2019b、2024b等版本直接运行。资源包共89个文件，包含30个m源码、19个mat数据、15个bmp仿真曲线、2个slx/Simulink模型及14个zbak备份、README说明等，整体16.53MB，已有43人学习下载。通过随附案例数据集与仿真图像，读者可深入理解EKF在非线性耦合系统中的状态估计实现、控制律调试与结果分析，涵盖姿态误差、轨道偏差、控制力矩等关键指标；完整源码和注释为后续修改扩展提供基础，可直接支撑课程项目或学位论文的仿真平台，是航天器控制教学与科研实践的高性价比工具。

1. 基于EKF的航天器姿态轨道耦合控制：这个问题为什么值得自己做一遍

做航天器GNC的人大概率遇到过这个场景：轨道机动时推力器点火，星体被偏置力矩顶歪；姿态机动时推力方向跟着偏，轨道又被扰动。分开设计的姿态环和轨道环在仿真里各自收敛，一联调就互相甩锅。把姿态和轨道放进同一个状态向量做EKF估计，再用耦合补偿的控制律收回来，是解决这个问题的正路。这篇笔记从耦合建模、EKF实现、LQR控制律到Matlab代码一步步拆，适合正在做预研或毕业设计的工程师，看完可以直接把代码跑起来。

2. 姿态轨道耦合动力学建模：状态方程、坐标约定与Matlab里的离散化

2.1 三个必须进模型的耦合项

航天器在轨的工作点通常是“姿态对地定向+轨道保持”，这种情况下姿态和轨道之间的耦合相对弱，把两个回路分开设计还能勉强工作。一旦进入轨道机动、交会接近或者大角度姿态机动场景，下面三个耦合项就不能忽略了。

第一，推力矢量与质心不重合。每个推力器都有安装位置和方向，推力并不严格通过质心，会产生固定力臂的干扰力矩 M_dist = d × F。这个力矩在轨道机动时持续存在，姿态控制系统必须实时补偿，否则星体姿态被推跑。

第二，姿态偏差会改变推力方向。推力指令在体坐标系下表达，如果姿态有误差 δθ，惯性系里实际推力方向就和期望方向差了 δθ。对轨道控制来说这会直接造成速度增量方向偏差，是典型的“姿态误差转化为轨道误差”。

第三，重力梯度力矩与轨道位置相关。非球形引力场里，轨道高度和经纬度变化都会改变重力梯度力矩，而姿态控制又把这种力矩当干扰。耦合趋势在低轨道、大惯量差的卫星上更明显。把这三个耦合项写进模型，EKF和控制律才能看到“一个卫星”而不是“两个独立回路”。

2.2 状态向量为什么是13维：笛卡尔坐标与四元数的选择

常用状态向量取 x = [r; v; q; ω]，其中 r、v 是惯性系下的位置和速度（各3维），q 是星体姿态四元数（4维），ω 是体坐标系角速度（3维），合计13维。轨道用笛卡尔坐标而不是轨道根数，是因为EKF预测方程在笛卡尔坐标下写起来直观、雅可比好算；轨道根数在接近圆轨道时存在奇异，没必要给自己找麻烦。

四元数有单位模约束，不能像普通向量那样做加权平均和协方差运算。这也是后续用误差状态EKF（MEKF）而不是直接状态EKF的前提。用欧拉角做状态可以绕开约束，但大角度机动时会遇到万向锁，姿态估计直接翻车，工程上基本没人拿欧拉角跑EKF。四元数这里统一采用标量在前约定 [q0; q1; q2; q3]，与Matlab自带工具包的顺序不一致，跑之前用单位旋转验证一下。

2.3 耦合动力学Matlab函数：从惯性系到体坐标系的完整代码

function xdot = coupled_dynamics(x, u, p) % 航天器姿态轨道耦合动力学 % x = [r(3); v(3); q(4); w(3)] % u = [F(3); M(3)] 惯性系推力(N)、惯性系控制力矩(N*m) % p = 参数结构体: mu, J, d_cm(推力作用点相对质心) r = x(1:3); v = x(4:6); q = x(7:10); w = x(11:13); q = q / norm(q); % 四元数保持单位模 R = quat2dcm(q); % 体坐标->惯性坐标，按工具包约定调整 F_body = R' * u(1:3); % 推力转体坐标系 M_body = R' * u(4:6) + cross(p.d_cm, F_body); % 轨道: 二体引力(可扩展J2项) g = -p.mu * r / norm(r)^3; % 姿态运动学: qdot = 1/2 * Omega(w) * q Omega = [0, -w(3), w(2); w(3), 0, -w(1); -w(2), w(1), 0]; qdot = 0.5 * [-q(2:4)' * w; q(1)*eye(3)*w + Omega * q(2:4)]; % 姿态动力学: J wdot + w x (J w) = M_body wdot = p.J \ (M_body - cross(w, p.J * w)); xdot = [v; g; qdot; wdot]; end

逻辑说明：代码分四段。轨道加速度目前只用了二体引力，需要J2摄动时在 g 那一行追加即可，不影响EKF接口。姿态运动学用的是四元数微分方程，其中 Omega 矩阵实现叉乘运算。姿态动力学里 M_body 除了控制力矩还加了 cross(p.d_cm, F_body)，这就是推力偏置力矩，是整个“耦合”在模型里的具体体现，删掉这一项后面所有控制补偿都无从谈起。

参数说明：p.mu 是地球引力常数，常用值 3.986004418e14 m^3/s^2；p.J 是 3×3 转动惯量矩阵；p.d_cm 是推力作用点相对质心的位置向量，取值建议在几厘米到几十厘米量级——太小会让偏置力矩淹没在数值噪声里，太大则质心标定可能有问题。实际任务里这个值来自地面质量特性测试，误差通常在2mm以内，仿真里给固定值即可。

2.4 用RK4把连续动力学离散成仿真步

EKF预测步需要求解状态转移，仿真主循环也需要对动力学积分。固定步长四阶Runge-Kutta是航天仿真最常用的做法，在秒级步长时精度足够、计算量可控。

function x_next = rk4_step(dynfun, x, u, p, dt) % 固定步长RK4：传播一个仿真步 k1 = dynfun(x, u, p); k2 = dynfun(x + dt/2*k1, u, p); k3 = dynfun(x + dt/2*k2, u, p); k4 = dynfun(x + dt*k3, u, p); x_next = x + dt/6 * (k1 + 2*k2 + 2*k3 + k4); end

RK4截断误差是 O(dt^5)，对LEO轨道秒级步长完全够用。注意 u 在整个步长内保持不变，这是零阶保持假设，如果控制律更新频率低于动力学积分频率，就需要在主循环里管理好几个时钟，后面第4章的主循环会演示怎么处理。大偏心率轨道或者近地点剧烈机动时固定步长会吃亏，解决办法是改 ode45 跑真值、EKF预测步仍然固定步长，这一点在第5章避坑详细展开。

3. EKF状态估计设计：误差状态、预测步与观测更新的Matlab实现

3.1 误差状态EKF：回避四元数约束的工程做法

直接对13维状态跑EKF会遇到数学问题：四元数有单位模约束，EKF的预测和更新都假设状态服从高斯分布，加入四元数后分布不再高斯，协方差更新会把单位模约束破坏掉。预测完归一化一次、更新完再归一化一次，短期跑起来还行，一旦观测噪声变大或机动增强，P矩阵就会失去正定性，滤波性能断崖式下降。

工程标准做法是误差状态EKF，主状态仍用13维，但P矩阵只作用在12维误差状态 δx = [δr; δv; δθ; δω] 上。姿态误差用三维角度向量 δθ 而不是四元数增量，协方差更新始终在无约束向量空间里进行，四元数只在主状态里传播和注入，约束与更新分开，既满足高斯假设，又保住四元数几何结构。

误差状态EKF又叫MEKF（Multiplicative EKF），因为姿态误差用四元数乘法定义：真实姿态 q = δq ⊗ q_pred，δq 是小角度四元数，与 δθ 的关系是 δq ≈ [1; δθ/2]。航天工程里基本都用这套。

3.2 预测步实现：数值雅可比是偷懒但稳妥的选择

误差状态EKF预测分两步：先让主状态沿非线性动力学走一步，再让误差状态协方差沿线性化矩阵传播。线性化矩阵 F = ∂f_err/∂δx 理论上可以推解析雅可比，但姿态轨道耦合下推导容易出错、模型一改就要重推，我更常用数值雅可比。差分变量是12维误差状态，四元数方向做扰动时要先转成小角度四元数再乘到主状态上，这是和普通数值差分最大的区别。

function [x_pred, P_pred] = mekf_predict(x, P, u, p, dt, Qd) % MEKF预测步: 标称状态传播 + 误差状态协方差传播 % Qd是离散化后的过程噪声协方差(12x12) f = @(xx) rk4_step(@coupled_dynamics, xx, u, p, dt); % 1. 标称状态传播 x_pred = f(x); x_pred(7:10) = x_pred(7:10) / norm(x_pred(7:10)); % 2. 对误差状态做中心差分求雅可比 F = zeros(12, 12); eps_j = 1e-6; for i = 1:12 dxe = zeros(12, 1); dxe(i) = eps_j; dxm = zeros(12, 1); dxm(i) = -eps_j; dx_after_e = propagate_error(dxe, x, x_pred, f); dx_after_m = propagate_error(dxm, x, x_pred, f); F(:, i) = (dx_after_e - dx_after_m) / (2*eps_j); end P_pred = F * P * F' + Qd; end function dx_after = propagate_error(dx, x, x_pred, f) % 在标称状态x处加误差扰动, 传播后再取回误差状态 xp = x; xp(1:6) = xp(1:6) + dx(1:6); % 位置/速度直接加扰动 dq = angle_to_quat(dx(7:9)); % 姿态误差角 -> 小角度四元数 xp(7:10) = quat_mult(dq, xp(7:10)); xp(11:13) = xp(11:13) + dx(10:12); xp_pred = f(xp); xp_pred(7:10) = xp_pred(7:10) / norm(xp_pred(7:10)); dx_after = zeros(12, 1); dx_after(1:6) = xp_pred(1:6) - x_pred(1:6); dx_after(7:9) = small_angle(xp_pred(7:10), x_pred(7:10)); dx_after(10:12) = xp_pred(11:13) - x_pred(11:13); end

逻辑说明：主状态传播只调用一次 rk4_step，中心差分里每次循环调用 propagate_error 两次，12个维度一共24次动力学积分，步长内计算量可接受。函数 angle_to_quat 把三维误差角转成小角度四元数，quat_mult 做四元数乘法，small_angle 取两个四元数之间的误差角向量，这三个自定义函数各自十几行，按前面约定的四元数顺序实现即可。

参数说明：差分步长 eps_j 选1e-6，对SI单位制合适；如果状态里混入了角度量级差异很大的变量，按各分量单独定步长更稳。Qd 是12×12过程噪声协方差，初值可按下表量级给，然后根据NEES检验微调。

3.3 观测更新：GPS、星敏感器与陀螺的H矩阵和残差

观测更新的核心是把残差投影到误差状态空间。位置观测残差直接是位置差，角速度观测残差直接是角速度差，姿态观测残差要先算测量四元数与预测四元数的误差四元数，再取小角度向量，不能直接做四元数减法。

function [x_upd, P] = mekf_update(x_pred, P, z, obs_type, R) % MEKF更新步: 支持 r(位置) / q(四元数姿态) / w(角速度) 三类观测 % z: 观测向量, R: 对应观测噪声协方差 switch obs_type case 'r' H = [eye(3), zeros(3,9)]; y = z - x_pred(1:3); case 'q' dq = quat_mult(z, quat_conj(x_pred(7:10))); if dq(1) < 0, dq = -dq; end % 消去符号歧义 y = 2 * dq(2:4); % 小角度姿态误差 H = [zeros(3,6), eye(3), zeros(3,3)]; case 'w' H = [zeros(3,9), eye(3)]; y = z - x_pred(11:13); end S = H * P * H' + R; K = P * H' / S; dx = K * y; % 误差状态注入主状态 x_upd = x_pred; x_upd(1:6) = x_upd(1:6) + dx(1:6); x_upd(11:13) = x_upd(11:13) + dx(10:12); dth = dx(7:9); if norm(dth) > 1e-12 dq_upd = [cos(norm(dth)/2); sin(norm(dth)/2) * dth/norm(dth)]; x_upd(7:10) = quat_mult(dq_upd, x_pred(7:10)); x_upd(7:10) = x_upd(7:10) / norm(x_upd(7:10)); end P = (eye(12) - K * H) * P; P = (P + P') / 2; % 强制对称 end

逻辑说明：P 是12×12，H 按观测类型取对应行，K 维度随之变化。姿态观测残差 y = 2*dq(2:4) 用的是小角度近似，残差超过5度时建议换成 atan2 形式的精确公式。误差状态注入时，姿态误差角要转回四元数再乘到预测四元数上，这个顺序不能颠倒。

参数说明：R 矩阵按传感器手册标定，星敏感器一般给 (1e-4)^2 rad^2 量级，GPS位置给 1^2 m^2，陀螺角速度给 (1e-4)^2 rad^2/s^2。注意星敏给出的姿态通常已经包含安装矩阵校准，仿真里做测量生成时也要先转成同一坐标系。

3.4 Q和R整定：这不是调平滑度的旋钮

Q和R整定是EKF使用里最像玄学的部分。原则是：R描述传感器噪声的物理精度，应该通过标定或手册确定，不要乱动；Q描述模型误差的大小，模型漏了什么就把对应分量Q调大。仿真里常见错误是把Q当“滤波强度”旋钮，Q调得越大曲线越平滑，但真实误差反而变大。

我一般这样起步：先用开环真值仿真（控制量给定、状态真值已知）单独验证EKF，Q初值按第3.2节表格给，跑一次蒙特卡洛看平均NEES。NEES偏低说明滤波器过自信，把Q调大；NEES偏高说明过程噪声给大了，把Q调小。调参调到NEES落进置信区间后，再闭环接控制律，此时如果曲线毛刺大，优先检查R是不是给小了，而不是继续动Q。

4. 姿态轨道耦合控制律设计：LQR反馈、前馈补偿与Matlab主循环

4.1 控制目标与误差状态定义

控制目标是让卫星跟踪一条期望轨道和期望姿态。期望轨道通常是机动目标轨迹，期望姿态要么对地定向，要么保持推力方向在惯性系下固定。误差状态取 δx = [δr; δv; δθ; δω]，与EKF的误差状态一致，控制律直接吃估计器输出的误差。

注意位置误差 δr 是在轨道控制坐标系（常见取 LVLH 当地轨道坐标系）下定义，而姿态误差 δθ 是在体坐标系下定义，两者混在一个向量里没问题，但反馈增益K的坐标一定要对应好。仿真里如果真值和估计值都在惯性系传播，控制律内部要先做一次坐标旋转，把惯性系误差转到控制坐标系，再乘K。

4.2 LQR线性化模型：C-W方程加刚体姿态方程

LQR需要一个线性时不变模型。轨道部分在圆轨道工作点用Clohessy-Wilshire方程，姿态部分在零角速度工作点取欧拉方程小扰动。两部分做分块对角合成，耦合项当作扰动由反馈裕度吸收，这是工程简化做法，效果靠闭环仿真验证。

function A = build_linear_model(p) % 12阶线性模型: 轨道C-W + 姿态小扰动 n = p.n; % 轨道角速度 rad/s A_cw = [0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 3*n^2 0 0 0 2*n 0; 0 0 0 -2*n 0 0; 0 0 -n^2 0 0 0]; % 工作点取零角速度, 重力梯度力矩当外部扰动 A_att = [zeros(3), eye(3); zeros(3), zeros(3)]; A = blkdiag(A_cw, A_att); end function K = design_lqr(p) % LQR权重: Bryson规则按物理容差给初值 A = build_linear_model(p); B = [zeros(3,3), zeros(3,3); eye(3)/p.m, zeros(3,3); zeros(3,3), zeros(3,3); zeros(3,3), inv(p.J)]; r_tol = 10; % 位置误差容差 10 m v_tol = 0.5; % 速度误差容差 0.5 m/s th_tol = 0.01; % 姿态误差容差 0.01 rad w_tol = 0.01; % 角速度误差容差 0.01 rad/s F_max = 20; % 最大推力 20 N M_max = 0.2; % 最大控制力矩 0.2 N*m Q_ctrl = diag([1/r_tol^2*ones(3,1); 1/v_tol^2*ones(3,1); ... 1/th_tol^2*ones(3,1); 1/w_tol^2*ones(3,1)]); R_ctrl = diag([1/F_max^2*ones(3,1); 1/M_max^2*ones(3,1)]); [K, ~, ~] = lqr(A, B, Q_ctrl, R_ctrl); end

逻辑说明：C-W方程前三行是运动学，后三行是动力学，包含 3n^2 径向项和 2n 科里奥利耦合项，这是圆轨道相对运动的固有特性，不能删。B矩阵的控制输入是控制坐标系下的推力与力矩，与第2章动力学函数的u接口一致。

参数说明：Bryson规则用物理容差倒数的平方作为权重初值，目的是让不同量纲的状态贡献归一化。r_tol、v_tol、th_tol、w_tol 表示你允许的误差幅度，F_max、M_max 表示执行机构极限。这一组设置完，K矩阵的量级基本合理，后续按仿真表现微调。

4.3 前馈补偿与执行机构分配：耦合控制的关键一笔

LQR反馈算出的控制量需要叠加前馈才能处理耦合。推力要跟踪期望加速度，同时推力偏置会给姿态一个干扰力矩，姿态控制必须预先抵消它。这就是耦合控制里最关键的“前馈补偿”一笔。

function [F_cmd, M_cmd] = coupled_lqr_correction(err_x, ref, p, K) % err_x: [δr; δv; δθ; δω] 控制坐标系下 % ref : 期望加速度 a_des(3) 等参考量 % 反作用反馈 u_corr = -K * err_x; F_corr = u_corr(1:3); M_corr = u_corr(4:6); % 轨道机动前馈: 期望加速度直接转推力 F_ff = p.m * ref.a_des; F_cmd = F_corr + F_ff; % 推力偏置力矩前馈补偿 M_ff = cross(p.d_cm, F_cmd); M_cmd = M_corr - M_ff; % 执行机构饱和 F_cmd = sat_vec(F_cmd, p.F_max); M_cmd = sat_vec(M_cmd, p.M_max); end

逻辑说明：M_ff = d × F 是推力不通过质心产生的干扰力矩，姿态控制要抵消它，所以从反馈力矩里减去。符号约定要跟第2章动力学里 M_body 一致，否则补偿变成叠加，姿态发散得更快。饱和函数 sat_vec 按向量模长限制，超出上限时等比例收缩而不是逐分量截断，避免改变推力方向。

参数说明：期望加速度 ref.a_des 来自轨道制导层，常见做法是跟踪一个机动规划器输出的轨迹。d_cm 在这里同时出现在动力学模型和控制器里，如果实际值和模型值不一致，控制效果会变差，这正好引出第5章的避坑案例。

4.4 主仿真循环：把估计、控制、积分三个频率串起来

dt_sim = 0.01; % 动力学积分 100 Hz dt_est = 0.1; % EKF估计 10 Hz dt_ctrl = 0.1; % 控制 10 Hz T_end = 600; t = 0; x_true = x0; x_est = x0; P = P0; u_cmd = zeros(6,1); for k = 1 : T_end / dt_sim % 1. 估计周期 if mod(t, dt_est) == 0 z = gen_measurement(x_true, R); % 产生观测并加噪声 [x_est, P] = mekf_predict(x_est, P, u_cmd, p, dt_est, Qd); [x_est, P] = mekf_update(x_est, P, z, 'r', R_r); [x_est, P] = mekf_update(x_est, P, z_q, 'q', R_q); end % 2. 控制周期 if mod(t, dt_ctrl) == 0 err_x = get_control_error(x_est, x_ref(t), p); [F_cmd, M_cmd] = coupled_lqr_correction(err_x, ref(t), p, K); end % 3. 仿真积分 x_true = rk4_step(@coupled_dynamics, x_true, [F_cmd; M_cmd], p, dt_sim); t = t + dt_sim; log_data(t, x_true, x_est, F_cmd, M_cmd); end

逻辑说明：估计频率和控制频率可以不同，这里都取10Hz。EKF预测步的 dt 是0.1s，但动力学积分步长是0.01s，两者严格保持一致，不要用积分步长去做预测。观测生成要在估计时刻进行，控制量用的是上一个控制周期的值，也就是零阶保持。

5. 避坑：EKF耦合系统里的四个翻车现场与排查方法

5.1 P矩阵失去正定性：现象、原因与解法

现象：仿真跑着跑着，P矩阵对角线出现负值，卡尔曼增益变成奇怪的数值，估计值突然跳变。

原因：直接状态EKF把四元数当作普通向量更新，协方差更新破坏了单位模约束；即使做了归一化，P矩阵在强观测下也会迅速失去正定性。另一个常见诱因是数值雅可比的差分步长不适合量级差异大的状态。

解决：切换到误差状态EKF，让P只作用于12维误差状态。同时每个观测更新完做一次 P = (P+P')/2 强制对称，必要时加微小对角项保证正定。如果不想换框架，至少要在预测和更新后都归一化四元数，但这不是长久之计。

5.2 姿态残差为0但滤波仍然漂移：隐藏的四元数符号歧义

现象：q 观测残差一直是零向量，EKF的姿态估计误差却持续增长，位置和角速度都很正常。

原因：四元数 q 和 -q 表示同一个姿态。如果测量生成时四元数符号翻转，姿态残差计算里 dq 接近 180° 旋转，小角度近似失效，或者说残差被错误地算成接近零。Matlab自带工具包某些函数对四元数符号敏感，很多人在这里浪费过一整天。

解决：在姿态残差计算里判断 dq(1)，小于零就取负，强制把符号唯一的表示挑出来。代码已在第3.3节给出。另一个辅助手段是观测更新前先打印 dq 的四元数模长和第一个分量，人工确认残差量级在预期范围内。

5.3 推力偏置力矩正反馈：补偿符号搞反的经典翻车

现象：闭环仿真里轨道控制收敛，姿态误差越来越大，检查控制器逻辑没发现明显bug。

原因：前馈补偿 M_ff 的符号和动力学模型里 M_body 的符号约定不一致。动力学里是 M_body = M_cmd + d×F，控制器里如果写成 M_cmd = M_corr + M_ff，干扰力矩变成两倍而不是抵消，姿态必然发散。

解决：把动力学和控制器里的力矩符号写在同一个注释块里，仿真前做一个纯开环检查：常值推力下，控制器只给 M_corr = 0，M_cmd = -d×F，看姿态是否保持稳定。这一步验证通过再闭环，符号问题基本绝迹。

5.4 大偏心率轨道固定步长积分发散：预测步的按需做

现象：圆形轨道仿真一切正常，换成椭圆轨道后EKF速度估计发散，真值轨迹也出现可疑振荡。

原因：RK4固定步长在近地点速度大、状态变化快时截断误差急剧增大，EKF预测步跟着把错误当成过程噪声吸收。另一个隐藏问题是数值雅可比用固定 eps_j=1e-6，对近地点位置量级变化很大的状态不够自适应。

解决：真值生成用 ode45 变步长积分，EKF预测步继续用固定步长但缩短到近地点步长，比如0.01s；或者对差分步长按分量自适应，取 eps_j = 1e-6 * max(1, abs(x(i)))。不要用同一套固定步长硬跑椭圆轨道，这是血泪经验。

6. 用蒙特卡洛与NEES检验滤波器的可信度：验证技巧与进阶做法

EKF调完参数后，判断滤波器是否可信的标准不是姿态曲线漂不漂亮，而是NEES（Normalized Estimation Error Squared）。NEES的定义是 x_err^T P^{-1} x_err / nx，其中 x_err 是估计误差，P 是滤波器输出的协方差，nx 是误差状态维数12。对每一次蒙特卡洛仿真，逐时刻计算NEES，再对多次仿真的同一时刻做平均，得到平均NEES。

function nees_mean = calc_nees(x_err_all, P_all) % x_err_all: nx-by-N-by-M 误差状态序列 % P_all: nx-by-nx-by-N 协方差序列(每次蒙特卡洛相同) % 平均NEES逐时刻计算 [nx, N, M] = size(x_err_all); nees_t = zeros(N, M); for m = 1:M for k = 1:N xk = x_err_all(:,k,m); nees_t(k,m) = xk' / P_all(:,:,k) * xk / nx; end end nees_mean = mean(nees_t, 2); end

判定标准：12维误差状态，95%置信区间对应的平均NEES约在0.72到1.32之间。平均NEES远大于1.3说明滤波器过于自信，P给得太小，估计误差比滤波器声称的大得多；平均NEES远小于0.7说明保守，P偏大，滤波器没有充分利用观测信息。这里不能只盯曲线，否则滤波器的过自信会在真实任务里变成灾难。

验证流程我一般分成三步。第一步，开环验证：控制量固定或按脚本给定，状态真值来自仿真，EKF只吃观测，跑20次蒙特卡洛看平均NEES，同时观察位置和姿态误差的均值是否在零附近。第二步，注入参数失配：把模型里的 d_cm 偏移2mm、J 矩阵加5%误差，再看NEES是否仍在区间内。失配后NEES轻微上升是正常的，如果急剧超限说明Q给得太小，模型误差没被编码进去。第三步，闭环保验证：接上第4章LQR，跑完整轨道机动场景，检查轨道重访误差和姿态残差是否收敛在容差内。

一个提升效率的技巧：把 Qd 和 R 的整定写成脚本函数，蒙特卡洛结果自动返回平均NEES的均值与区间外比例，用表格对比不同参数组。这样调参从“看曲线猜”变成“量化决策”。我做这类系统时的习惯是先把开环NEES调到带内再闭环保验证，不然两个环互相怪罪，翻车概率高得多。希望帮到你。

本文还有配套的精品资源，点击获取