简介一套面向航天器动力学与控制研究的姿态-轨道耦合控制系统Matlab实现基于扩展卡尔曼滤波EKF算法旨在解决在轨任务中姿态稳定与轨道精确跟踪的非线性状态估计问题适合高年级本科生、研究生及科研人员用于课程项目、专题研究与毕业设计。资源包共89个文件、16.53MB含30个m脚本、19个mat数据文件、15个bmp仿真图、2个slx模型及xml/zbak等辅助文件m脚本为核心算法与功能模块mat为仿真中间数据bmp为结果对比图支持MATLAB 2014a、2019b、2024b直接运行免去数据预处理。系统采用模块化参数化编程代码注释详尽配套可直接执行案例数据集用户可灵活调整参数以适配轨道维持、交会对接、行星着陆等场景。已有41人学习。通过实际仿真与参数调试读者既能理解EKF在强耦合非线性系统中的滤波流程与实现细节也能掌握航天器控制系统从建模、算法设计到结果分析的一体化方法可直接支撑课程设计与毕业设计。1. EKF耦合仿真里的第一个坑协方差匹配在没有测量噪声的六自由度仿真里任何一种反馈控制器都能把姿态误差压得很低可一旦把EKF加入回路最先暴露问题的是状态估计的协方差匹配而不是控制增益。基于EKF的航天器姿态轨道耦合控制系统把四元数姿态、体轴角速度和轨道位置速度放到同一个13维状态向量里用扩展卡尔曼滤波做实时估计再由PD或LQR闭环驱动最终在Simulink中输出姿态误差、角速度误差和控制力矩曲线。包内从Motionfun.m、X_Matrix.m到EKF.m、measure.m、Torquer.m覆盖了建模、估计、控制、后处理全链路支持MATLAB 2014a/2019b/2024b直接运行。这个工程适合毕业设计、控制课程大作业以及想研究EKF参数整定对控制精度影响的工程师。2. 姿态轨道耦合系统的状态方程建模EKF的性能上限由模型精度决定模型偏差会被状态估计吃进去再吐出来。因此第一步不是调滤波器而是把姿态、轨道以及它们之间的耦合项写到同一个微分方程组里。2.1 四元数姿态运动学与角速度外推姿态表示用四元数而不是欧拉角原因很直接欧拉角在俯仰±90°附近存在奇异点而四元数用四个参数描述旋转没有奇异点代价是多一个归一化约束。工程里的EulerToQ.m负责把欧拉角转成四元数QtoEuler.m负责反向输出真正参与状态递推的是EulerdotToOmega.m它实现的运动学方程为function qdot EulerdotToOmega(q, omega) q0 q(1); qv q(2:4); Q [-qv; q0*eye(3) skew(qv)]; qdot 0.5 * Q * omega; end function s skew(v) s [ 0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0 ]; endskew函数构造叉乘反对称矩阵是刚体动力学里的高频工具。注意四元数递推后必须保持单位范数否则后续姿态误差计算会带进不存在的旋转。我在跑仿真时通常每一步都把q(1:4)重新归一化一次这个操作放在EKF更新之后尤其重要因为滤波增益会破坏四元数的单位长度约束。2.2 刚体姿态动力学与重力梯度力矩姿态动力学由Dynamicfun.m和Dynamicfun1.m两个版本给出参数化编程的核心是转动惯量J、干扰力矩和控制器输出。比较常见的形式是function xdot Dynamicfun(t, x, u, param) q x(1:4); omega x(5:7); J param.J; T_grav gravity_gradient(x(8:10), q, param); omega_dot J \ ( -cross(omega, J*omega) u T_grav ); q_dot EulerdotToOmega(q, omega); xdot [q_dot; omega_dot]; end重力梯度力矩是姿态轨道最典型的耦合项轨道位置r通过1/|r|^3进入力矩公式而姿态四元数决定哪一维星体主轴对准地心矢量。如果忽略这个耦合EKF在低轨场景下估计出的姿态会持续偏移尤其是沿轨道法线方向的稳态偏差很难用陀螺和星敏的加权消掉。Dynamicfun1.m与Dynamicfun.m的差别通常在于是否包含推力偏心带来的扰动项跑仿真时可以对比两个版本对估计误差的影响。2.3 轨道运动两体模型与Motionfun轨道动力学在Motionfun.m和Motionfun1.m中实现基础是两体问题考虑J2摄动时会在加速度项里加一个与纬度辐角相关的修正。这里给出一个简化的二体递推示意function x_next Motionfun(x, dt, mu) r x(1:3); v x(4:6); r_norm norm(r); a -mu / r_norm^3 * r; x_next zeros(6,1); x_next(1:3) r v * dt; x_next(4:6) v a * dt; end实际工程不会用这种显式欧拉我在main.m里用的是四阶Runge-Kutta步长根据轨道周期设定在0.1s左右。这里写成显式欧拉是为了把力和加速度的关系交代清楚控制力不仅改变轨道其推力偏心会造成姿态干扰力矩所以耦合是双向的。EKF状态向量同时估计姿态四元数、角速度和轨道位置速度就能通过量测冗余将两个子系统的误差相互修正。2.4 状态向量线性化与X_Matrix矩阵EKF需要用到状态转移矩阵的雅可比F包内X_Matrix.m就是干这个的。它接受当前状态x和参数param输出连续线性化矩阵F再通过一阶指数映射得到离散状态转移矩阵Phi。文件列表中还有Ax.m、Ay.m、Az.m我理解是分别对三个轴向扰动求数值雅可比的辅助脚本常见做法是以这些脚本为基线用中心差分验证解析形式的正确性。function Phi discretize_F(F, dt) % 一阶近似适合步长远小于系统最小时间常数 Phi eye(size(F)) F * dt; end一阶近似在小步长下够用但步长超过0.5s时应该用expm或二阶修正。状态向量统一为13维表2.1给出了我常用的排序方式这个顺序必须和X_Matrix.m里的一致否则EKF的协方差矩阵全是错位。表2.1 耦合状态向量定义通道状态含义单位1-4q姿态四元数 [q0, q1, q2, q3]-5-7ω体坐标系角速度rad/s8-10r惯性系轨道位置m11-13v惯性系轨道速度m/s这里可以同时看到为什么不能直接使用欧拉角作为状态欧拉角不仅奇异而且运动学方程含三角函数线性化后雅可比在姿态大范围变化时误差极大。四元数虽然约束复杂但雅可比形式稳定配合归一化处理非常成熟。3. EKF算法设计与量测更新实现这一章进入核心。EKF在航天器中的应用不是把标准公式抄一遍而要在四元数约束、量测冗余和实时性之间做平衡。3.1 连续-离散EKF的流程划分系统模型是连续的微分方程控制器的更新是离散的因此采用连续-离散EKF预测步用RK4把状态和协方差从t_k推到t_{k1}更新步用离散量测修正。协方差预测公式为P_pred Phi * P * Phi QdQd是过程噪声协方差矩阵的离散化结果工程上常用零阶保持输入假设把连续噪声谱矩阵S乘以步长dt得到。包内EKF.m文件直接实现了这个流程但没有把状态预测单独拆出来而是要求外部先做一次动力学积分再把积分结果传给EKF更新。3.2 量测模型与measure.m量测方程描述“状态到观测”的映射包内的measure.m负责生成带噪声的量测数据。它模拟了星敏感器四元数、陀螺角速度和轨道定位r、v三类传感器量测模型可写成线性形式function z measure(t, x_true, R_std) q x_true(1:4); z(1:4) q / norm(q) R_std.q * randn(4,1); z(5:7) x_true(5:7) R_std.w * randn(3,1); z(8:10) x_true(8:10) R_std.r * randn(3,1); z(11:13) x_true(11:13) R_std.v * randn(3,1); end注意四元数量测噪声不能直接加在四个分量上因为加完噪声后四元数不再满足归一化。一个常见做法是把噪声加在等效旋转矢量或修正罗德里格参数上再映射回四元数这里为了演示用randn直接加并归一化只做教学仿真没问题。实际工程中我会在星敏感器模型里保留圆锥误差的方差缩放系数让R矩阵更接近真实传感器参数。3.3 EKF核心更新步骤解析EKF.m中的核心更新段可以整理为以下结构function [x_upd, P_upd] ekf_core(x_pred, P_pred, z, Phi, H, Qd, R) % 预测协方差 P_pred Phi * P_pred * Phi Qd; % 新息协方差 S H * P_pred * H R; K P_pred * H / S; % 量测残差这里H是常矩阵可直接计算 innov z - H * x_pred; % 状态更新 x_upd x_pred K * innov; % 四元数保持单位范数 x_upd(1:4) x_upd(1:4) / norm(x_upd(1:4)); % Joseph形式协方差更新 I eye(numel(x_pred)); P_upd (I - K*H) * P_pred * (I - K*H) K*R*K; end为什么用Joseph形式因为标准形式(I - K*H)*P在数值上不保证对称性长时间运行后P矩阵会出现不对称特征值导致卡尔曼增益漂移。Joseph形式虽然计算量大一些但配合MATLAB的浮点运算足够稳定。另一个关键点是四元数量测残差如果真实姿态q和预测姿态q_pred的矢量部分符号相反四元数差会接近2而不是0这属于四元数的双映射问题。我的处理方式是先比较q和-q哪一个离预测值更近再计算残差。提示如果仿真中P矩阵出现非对称或特征值发散优先确认代码用的是不是Joseph形式而不是急着调小R矩阵。3.4 初始协方差与噪声参数整定initialize.m给出了P0和滤波器参数的初值。表3.1是我在这个工程中反复调过的一组起点。表3.1 EKF噪声参数推荐起点参数符号数值说明姿态过程噪声Q_q1e-8 * I(3)补偿模型误差太大会导致姿态闪烁角速度过程噪声Q_w1e-6 * I(3)陀螺角随机游走轨道位置过程噪声Q_r1e-4 * I(3)实际为推力误差等效加速度轨道速度过程噪声Q_v1e-6 * I(3)非保守力摄动姿态量测噪声R_q3e-8 * I(4)约0.01°星敏工程精度角速度量测噪声R_w(0.01°/s)^2陀螺测量白噪声位置量测噪声R_r100^2 * I(3)GPS绝对定位误差速度量测噪声R_v0.1^2 * I(3)GPS测速误差实际使用时Q和R要做一个“预白化”验证跑完仿真后检查新息序列的均值是否接近0、自相关是否接近delta函数。如果新息均值持续偏置不要先调Q而是检查量测模型是否漏了常值误差项。3.5 EKF发散的五种典型症状第一P矩阵变成非对称或负定用Joseph形式后基本能避免。第二新息序列持续同号说明模型偏差大于滤波器的“信任程度”需要增大Q或者检查雅可比。第三四元数分量数值跳变检查是否处理了双映射。第四协方差P收敛到零但误差依然很大说明系统不可观本工程的13维状态在量测完整时是可观的但删除某一路量测后会出现这种病态。第五控制力矩饱和导致输入突变EKF状态预测仍在按线性控制量推导可以在预测步把实际限幅后的控制量作为输入。4. 航天器姿态控制器设计与闭环仿真EKF只是眼睛控制器才是手臂。包内PD.m、PD1.m、PD2.m、LQR.m同时存在说明作者对比过几套控制方案。4.1 PD姿态控制律与误差四元数PD控制是实现姿态机动最直接的方法。工程中的做法是把当前姿态与目标姿态的误差四元数矢量部分作为比例项输入角速度误差作为微分项输入function T_cmd PD(q_err, omega_err, Kp, Kd) % q_err: 误差四元数由EKF估计姿态与目标姿态计算得到 % omega_err: 角速度误差 qe q_err(2:4); T_cmd -Kp * qe - Kd * omega_err; endKp和Kd不是随便选的。对于刚体卫星如果已知转动惯量J和期望频带ω_n可以按Kp Jω_n^2、Kd 2ζω_nJ来初定再在仿真里慢慢调。包内PD1.m、PD2.m应该是不同参数或不同执行机构条件下的变体测试时注意区分。PD控制的好处是每个轴独立缺点是大角度机动时四元数误差的非线性使稳定裕度下降所以有人用LQR把速率阻尼和姿态机动统一在一个状态反馈里。4.2 LQR状态反馈控制LQR.m文件通常封装了MATLAB的lqr函数function [K, S] LQR(A, B, Q_ctrl, R_ctrl) [K, S] lqr(A, B, Q_ctrl, R_ctrl); end这里的A、B来自在目标姿态处的线性化Q_ctrl是状态权重R_ctrl是控制权重。LQR的优点是可以直接对耦合模型做多变量设计把姿态和轨道误差的权重交叉项写进Q_ctrl这样控制器天然考虑了耦合影响。缺点是权重矩阵没有直观物理意义调参难度比PD大。我的建议是先用PD把基础回路跑通再切换LQR用PD参数倒推LQR的权重。4.3 Torquer执行器模型与控制分配Torquer.m的名字暗示执行器是磁力矩器但文件列表里还有steer.m可能也包含反作用飞轮或推力器模型。磁力矩器只能在与地磁场垂直的平面内产生力矩所以需要控制分配逻辑。一个典型的饱和限幅代码段function T_sat Torquer(T_cmd, T_max) % 整体限幅超出上限按比例缩放 n norm(T_cmd); if n T_max T_sat T_cmd / n * T_max; else T_sat T_cmd; end end这里用整体比例缩放而不是轴独立限幅保留了力矩方向避免磁力矩器产生沿地磁场方向的分量导致效率下降。执行器饱和会影响EKF预测所以仿真中要把T_sat反馈给状态预测模块而不是使用T_cmd。4.4 Model1.slx与M文件的集成方式Simulink模型Model1.slx是闭环仿真的主载体Model.slx是更早期的版本。通常会这样搭子系统轨道姿态动力学模块使用Level-2 M S-Function调用Dynamicfun.m和Motionfun.m控制器模块直接嵌入PD2.m的MATLAB Function或者调用LQR.m返回的增益EKF模块用Interpreted MATLAB Function调EKF.m等待输入量测和动力学积分结果结果输出模块把估计状态、量测值和时间戳输出到工作区供huitu.m绘图。集成时要注意数据类型的匹配Simulink的MATLAB Function模块默认输出double但如果把EKF.m写成S-Function需要在函数文件里正确设置输入输出端口数量。我第一次跑Model1.slx时遇到“无法从输入端口赋初值”的报错原因是EKF.m的状态向量维度写死为13而Simulink里的积分模块输出维度与状态维度不一致需要把状态维度与C函数原型统一。注意如果Simulink模型里的EKF模块输出NaN多半是状态向量维度被设为6改成13再检查输入端口是否接全。4.5 主脚本运行流程与结果解读运行顺序我一般是这样先运行initialize.m生成参数结构体param保存到nowheel.mat运行main.m启动闭环仿真仿真时长300sEKF更新步长0.1s仿真结束后运行analyse.m计算估计误差和控制精度用huitu.m / huitu2.m批量出图。main.m中关键的一段可以抽象为% 主循环简化示意 for k 1:N % 动力学积分 x_true rk4_step(x_true, T_cmd, dt_dyn, param); % 生成量测 z measure(t(k), x_true, R_std); % EKF预测更新 [x_est, P] ekf_predict_update(x_est, P, z, dt_est, param); % 控制器计算控制力矩 q_err quat_multiply(quat_inv(x_est(1:4)), q_target); omega_err x_est(5:7) - omega_target; T_cmd PD(q_err, omega_err, param.Kp, param.Kd); % 执行器限幅 T_cmd Torquer(T_cmd, param.T_max); end输出的图像里Q估计1.bmp和w估计.bmp是滤波器输出与真实值的对比dwd.bmp是角速度估计误差dQd.bmp和dQm.bmp分别是估计四元数误差和量测残差。看到dQm.bmp的高斯白噪声特性基本可以判断量测模型没问题dwd.bmp如果出现缓慢漂移优先检查陀螺噪声的R矩阵是否和实际模型一致。表4.1 结果曲线对应的定位文件名内容判定标准Q估计1.bmp估计四元数与真值对比稳态重合w量测.bmp带噪角速度量测波动范围符合R_wdwm.bmp角速度量测残差白噪声均值为0Tw.bmp控制力矩时间序列未持续饱和5. 把EKF调到稳定的三个验证技巧系统跑通之后离可靠还有一段路。下面这个技巧是我拆这个工程后最想分享的用蒙特卡洛仿真加3σ包络检验判断Q/R矩阵是否调对。把所有随机种子抽出来跑50次同一任务统计每一维估计误差与滤波器协方差标准差之间的归一化比率function ok monte_carlo_3sigma(N, seed) rng(seed); n 13; sigma_ratio zeros(n, N); for i 1:N [x_est, P, x_true] run_single_sim(); % 单次仿真 err x_true - x_est; sig sqrt(diag(P)); sigma_ratio(:, i) abs(err) ./ sig; end % 每维误差超过3σ的比例 over_ratio sum(sigma_ratio 3, 2) / N; % 1表示通过0表示某维超限 ok all(over_ratio 0.05); end如果某维度的over_ratio明显高于5%说明该通道的Q或R设置失配。举例来说角速度维度超限了调大Q_w姿态四元数维度超限调大Q_q同时检查四元数量测噪声R_q是否写小了。这里有一个容易踩的坑P矩阵是滤波器估计的协方差不是真实误差的均方值所以必须通过多次仿真统计后才能使用3σ判据单次仿真看不出来。如果要用stability.m做系统稳定性分析记住它生成的是线性化系统在平衡点的特征值分布只能帮助判断控制器增益是否安全不能验证EKF收敛性。EKF的收敛性验证还得靠蒙特卡洛包络。经过这一轮检验后再回去调PD/LQR参数就能把估计误差和控制精度分开审视不会出现“控制器增益改大反而精度下降”的错觉。本文还有配套的精品资源点击获取 EKF航天器姿态轨道耦合控制仿真：从建模到Simulink闭环
简介一套面向航天器动力学与控制研究的姿态-轨道耦合控制系统Matlab实现基于扩展卡尔曼滤波EKF算法旨在解决在轨任务中姿态稳定与轨道精确跟踪的非线性状态估计问题适合高年级本科生、研究生及科研人员用于课程项目、专题研究与毕业设计。资源包共89个文件、16.53MB含30个m脚本、19个mat数据文件、15个bmp仿真图、2个slx模型及xml/zbak等辅助文件m脚本为核心算法与功能模块mat为仿真中间数据bmp为结果对比图支持MATLAB 2014a、2019b、2024b直接运行免去数据预处理。系统采用模块化参数化编程代码注释详尽配套可直接执行案例数据集用户可灵活调整参数以适配轨道维持、交会对接、行星着陆等场景。已有41人学习。通过实际仿真与参数调试读者既能理解EKF在强耦合非线性系统中的滤波流程与实现细节也能掌握航天器控制系统从建模、算法设计到结果分析的一体化方法可直接支撑课程设计与毕业设计。1. EKF耦合仿真里的第一个坑协方差匹配在没有测量噪声的六自由度仿真里任何一种反馈控制器都能把姿态误差压得很低可一旦把EKF加入回路最先暴露问题的是状态估计的协方差匹配而不是控制增益。基于EKF的航天器姿态轨道耦合控制系统把四元数姿态、体轴角速度和轨道位置速度放到同一个13维状态向量里用扩展卡尔曼滤波做实时估计再由PD或LQR闭环驱动最终在Simulink中输出姿态误差、角速度误差和控制力矩曲线。包内从Motionfun.m、X_Matrix.m到EKF.m、measure.m、Torquer.m覆盖了建模、估计、控制、后处理全链路支持MATLAB 2014a/2019b/2024b直接运行。这个工程适合毕业设计、控制课程大作业以及想研究EKF参数整定对控制精度影响的工程师。2. 姿态轨道耦合系统的状态方程建模EKF的性能上限由模型精度决定模型偏差会被状态估计吃进去再吐出来。因此第一步不是调滤波器而是把姿态、轨道以及它们之间的耦合项写到同一个微分方程组里。2.1 四元数姿态运动学与角速度外推姿态表示用四元数而不是欧拉角原因很直接欧拉角在俯仰±90°附近存在奇异点而四元数用四个参数描述旋转没有奇异点代价是多一个归一化约束。工程里的EulerToQ.m负责把欧拉角转成四元数QtoEuler.m负责反向输出真正参与状态递推的是EulerdotToOmega.m它实现的运动学方程为function qdot EulerdotToOmega(q, omega) q0 q(1); qv q(2:4); Q [-qv; q0*eye(3) skew(qv)]; qdot 0.5 * Q * omega; end function s skew(v) s [ 0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0 ]; endskew函数构造叉乘反对称矩阵是刚体动力学里的高频工具。注意四元数递推后必须保持单位范数否则后续姿态误差计算会带进不存在的旋转。我在跑仿真时通常每一步都把q(1:4)重新归一化一次这个操作放在EKF更新之后尤其重要因为滤波增益会破坏四元数的单位长度约束。2.2 刚体姿态动力学与重力梯度力矩姿态动力学由Dynamicfun.m和Dynamicfun1.m两个版本给出参数化编程的核心是转动惯量J、干扰力矩和控制器输出。比较常见的形式是function xdot Dynamicfun(t, x, u, param) q x(1:4); omega x(5:7); J param.J; T_grav gravity_gradient(x(8:10), q, param); omega_dot J \ ( -cross(omega, J*omega) u T_grav ); q_dot EulerdotToOmega(q, omega); xdot [q_dot; omega_dot]; end重力梯度力矩是姿态轨道最典型的耦合项轨道位置r通过1/|r|^3进入力矩公式而姿态四元数决定哪一维星体主轴对准地心矢量。如果忽略这个耦合EKF在低轨场景下估计出的姿态会持续偏移尤其是沿轨道法线方向的稳态偏差很难用陀螺和星敏的加权消掉。Dynamicfun1.m与Dynamicfun.m的差别通常在于是否包含推力偏心带来的扰动项跑仿真时可以对比两个版本对估计误差的影响。2.3 轨道运动两体模型与Motionfun轨道动力学在Motionfun.m和Motionfun1.m中实现基础是两体问题考虑J2摄动时会在加速度项里加一个与纬度辐角相关的修正。这里给出一个简化的二体递推示意function x_next Motionfun(x, dt, mu) r x(1:3); v x(4:6); r_norm norm(r); a -mu / r_norm^3 * r; x_next zeros(6,1); x_next(1:3) r v * dt; x_next(4:6) v a * dt; end实际工程不会用这种显式欧拉我在main.m里用的是四阶Runge-Kutta步长根据轨道周期设定在0.1s左右。这里写成显式欧拉是为了把力和加速度的关系交代清楚控制力不仅改变轨道其推力偏心会造成姿态干扰力矩所以耦合是双向的。EKF状态向量同时估计姿态四元数、角速度和轨道位置速度就能通过量测冗余将两个子系统的误差相互修正。2.4 状态向量线性化与X_Matrix矩阵EKF需要用到状态转移矩阵的雅可比F包内X_Matrix.m就是干这个的。它接受当前状态x和参数param输出连续线性化矩阵F再通过一阶指数映射得到离散状态转移矩阵Phi。文件列表中还有Ax.m、Ay.m、Az.m我理解是分别对三个轴向扰动求数值雅可比的辅助脚本常见做法是以这些脚本为基线用中心差分验证解析形式的正确性。function Phi discretize_F(F, dt) % 一阶近似适合步长远小于系统最小时间常数 Phi eye(size(F)) F * dt; end一阶近似在小步长下够用但步长超过0.5s时应该用expm或二阶修正。状态向量统一为13维表2.1给出了我常用的排序方式这个顺序必须和X_Matrix.m里的一致否则EKF的协方差矩阵全是错位。表2.1 耦合状态向量定义通道状态含义单位1-4q姿态四元数 [q0, q1, q2, q3]-5-7ω体坐标系角速度rad/s8-10r惯性系轨道位置m11-13v惯性系轨道速度m/s这里可以同时看到为什么不能直接使用欧拉角作为状态欧拉角不仅奇异而且运动学方程含三角函数线性化后雅可比在姿态大范围变化时误差极大。四元数虽然约束复杂但雅可比形式稳定配合归一化处理非常成熟。3. EKF算法设计与量测更新实现这一章进入核心。EKF在航天器中的应用不是把标准公式抄一遍而要在四元数约束、量测冗余和实时性之间做平衡。3.1 连续-离散EKF的流程划分系统模型是连续的微分方程控制器的更新是离散的因此采用连续-离散EKF预测步用RK4把状态和协方差从t_k推到t_{k1}更新步用离散量测修正。协方差预测公式为P_pred Phi * P * Phi QdQd是过程噪声协方差矩阵的离散化结果工程上常用零阶保持输入假设把连续噪声谱矩阵S乘以步长dt得到。包内EKF.m文件直接实现了这个流程但没有把状态预测单独拆出来而是要求外部先做一次动力学积分再把积分结果传给EKF更新。3.2 量测模型与measure.m量测方程描述“状态到观测”的映射包内的measure.m负责生成带噪声的量测数据。它模拟了星敏感器四元数、陀螺角速度和轨道定位r、v三类传感器量测模型可写成线性形式function z measure(t, x_true, R_std) q x_true(1:4); z(1:4) q / norm(q) R_std.q * randn(4,1); z(5:7) x_true(5:7) R_std.w * randn(3,1); z(8:10) x_true(8:10) R_std.r * randn(3,1); z(11:13) x_true(11:13) R_std.v * randn(3,1); end注意四元数量测噪声不能直接加在四个分量上因为加完噪声后四元数不再满足归一化。一个常见做法是把噪声加在等效旋转矢量或修正罗德里格参数上再映射回四元数这里为了演示用randn直接加并归一化只做教学仿真没问题。实际工程中我会在星敏感器模型里保留圆锥误差的方差缩放系数让R矩阵更接近真实传感器参数。3.3 EKF核心更新步骤解析EKF.m中的核心更新段可以整理为以下结构function [x_upd, P_upd] ekf_core(x_pred, P_pred, z, Phi, H, Qd, R) % 预测协方差 P_pred Phi * P_pred * Phi Qd; % 新息协方差 S H * P_pred * H R; K P_pred * H / S; % 量测残差这里H是常矩阵可直接计算 innov z - H * x_pred; % 状态更新 x_upd x_pred K * innov; % 四元数保持单位范数 x_upd(1:4) x_upd(1:4) / norm(x_upd(1:4)); % Joseph形式协方差更新 I eye(numel(x_pred)); P_upd (I - K*H) * P_pred * (I - K*H) K*R*K; end为什么用Joseph形式因为标准形式(I - K*H)*P在数值上不保证对称性长时间运行后P矩阵会出现不对称特征值导致卡尔曼增益漂移。Joseph形式虽然计算量大一些但配合MATLAB的浮点运算足够稳定。另一个关键点是四元数量测残差如果真实姿态q和预测姿态q_pred的矢量部分符号相反四元数差会接近2而不是0这属于四元数的双映射问题。我的处理方式是先比较q和-q哪一个离预测值更近再计算残差。提示如果仿真中P矩阵出现非对称或特征值发散优先确认代码用的是不是Joseph形式而不是急着调小R矩阵。3.4 初始协方差与噪声参数整定initialize.m给出了P0和滤波器参数的初值。表3.1是我在这个工程中反复调过的一组起点。表3.1 EKF噪声参数推荐起点参数符号数值说明姿态过程噪声Q_q1e-8 * I(3)补偿模型误差太大会导致姿态闪烁角速度过程噪声Q_w1e-6 * I(3)陀螺角随机游走轨道位置过程噪声Q_r1e-4 * I(3)实际为推力误差等效加速度轨道速度过程噪声Q_v1e-6 * I(3)非保守力摄动姿态量测噪声R_q3e-8 * I(4)约0.01°星敏工程精度角速度量测噪声R_w(0.01°/s)^2陀螺测量白噪声位置量测噪声R_r100^2 * I(3)GPS绝对定位误差速度量测噪声R_v0.1^2 * I(3)GPS测速误差实际使用时Q和R要做一个“预白化”验证跑完仿真后检查新息序列的均值是否接近0、自相关是否接近delta函数。如果新息均值持续偏置不要先调Q而是检查量测模型是否漏了常值误差项。3.5 EKF发散的五种典型症状第一P矩阵变成非对称或负定用Joseph形式后基本能避免。第二新息序列持续同号说明模型偏差大于滤波器的“信任程度”需要增大Q或者检查雅可比。第三四元数分量数值跳变检查是否处理了双映射。第四协方差P收敛到零但误差依然很大说明系统不可观本工程的13维状态在量测完整时是可观的但删除某一路量测后会出现这种病态。第五控制力矩饱和导致输入突变EKF状态预测仍在按线性控制量推导可以在预测步把实际限幅后的控制量作为输入。4. 航天器姿态控制器设计与闭环仿真EKF只是眼睛控制器才是手臂。包内PD.m、PD1.m、PD2.m、LQR.m同时存在说明作者对比过几套控制方案。4.1 PD姿态控制律与误差四元数PD控制是实现姿态机动最直接的方法。工程中的做法是把当前姿态与目标姿态的误差四元数矢量部分作为比例项输入角速度误差作为微分项输入function T_cmd PD(q_err, omega_err, Kp, Kd) % q_err: 误差四元数由EKF估计姿态与目标姿态计算得到 % omega_err: 角速度误差 qe q_err(2:4); T_cmd -Kp * qe - Kd * omega_err; endKp和Kd不是随便选的。对于刚体卫星如果已知转动惯量J和期望频带ω_n可以按Kp Jω_n^2、Kd 2ζω_nJ来初定再在仿真里慢慢调。包内PD1.m、PD2.m应该是不同参数或不同执行机构条件下的变体测试时注意区分。PD控制的好处是每个轴独立缺点是大角度机动时四元数误差的非线性使稳定裕度下降所以有人用LQR把速率阻尼和姿态机动统一在一个状态反馈里。4.2 LQR状态反馈控制LQR.m文件通常封装了MATLAB的lqr函数function [K, S] LQR(A, B, Q_ctrl, R_ctrl) [K, S] lqr(A, B, Q_ctrl, R_ctrl); end这里的A、B来自在目标姿态处的线性化Q_ctrl是状态权重R_ctrl是控制权重。LQR的优点是可以直接对耦合模型做多变量设计把姿态和轨道误差的权重交叉项写进Q_ctrl这样控制器天然考虑了耦合影响。缺点是权重矩阵没有直观物理意义调参难度比PD大。我的建议是先用PD把基础回路跑通再切换LQR用PD参数倒推LQR的权重。4.3 Torquer执行器模型与控制分配Torquer.m的名字暗示执行器是磁力矩器但文件列表里还有steer.m可能也包含反作用飞轮或推力器模型。磁力矩器只能在与地磁场垂直的平面内产生力矩所以需要控制分配逻辑。一个典型的饱和限幅代码段function T_sat Torquer(T_cmd, T_max) % 整体限幅超出上限按比例缩放 n norm(T_cmd); if n T_max T_sat T_cmd / n * T_max; else T_sat T_cmd; end end这里用整体比例缩放而不是轴独立限幅保留了力矩方向避免磁力矩器产生沿地磁场方向的分量导致效率下降。执行器饱和会影响EKF预测所以仿真中要把T_sat反馈给状态预测模块而不是使用T_cmd。4.4 Model1.slx与M文件的集成方式Simulink模型Model1.slx是闭环仿真的主载体Model.slx是更早期的版本。通常会这样搭子系统轨道姿态动力学模块使用Level-2 M S-Function调用Dynamicfun.m和Motionfun.m控制器模块直接嵌入PD2.m的MATLAB Function或者调用LQR.m返回的增益EKF模块用Interpreted MATLAB Function调EKF.m等待输入量测和动力学积分结果结果输出模块把估计状态、量测值和时间戳输出到工作区供huitu.m绘图。集成时要注意数据类型的匹配Simulink的MATLAB Function模块默认输出double但如果把EKF.m写成S-Function需要在函数文件里正确设置输入输出端口数量。我第一次跑Model1.slx时遇到“无法从输入端口赋初值”的报错原因是EKF.m的状态向量维度写死为13而Simulink里的积分模块输出维度与状态维度不一致需要把状态维度与C函数原型统一。注意如果Simulink模型里的EKF模块输出NaN多半是状态向量维度被设为6改成13再检查输入端口是否接全。4.5 主脚本运行流程与结果解读运行顺序我一般是这样先运行initialize.m生成参数结构体param保存到nowheel.mat运行main.m启动闭环仿真仿真时长300sEKF更新步长0.1s仿真结束后运行analyse.m计算估计误差和控制精度用huitu.m / huitu2.m批量出图。main.m中关键的一段可以抽象为% 主循环简化示意 for k 1:N % 动力学积分 x_true rk4_step(x_true, T_cmd, dt_dyn, param); % 生成量测 z measure(t(k), x_true, R_std); % EKF预测更新 [x_est, P] ekf_predict_update(x_est, P, z, dt_est, param); % 控制器计算控制力矩 q_err quat_multiply(quat_inv(x_est(1:4)), q_target); omega_err x_est(5:7) - omega_target; T_cmd PD(q_err, omega_err, param.Kp, param.Kd); % 执行器限幅 T_cmd Torquer(T_cmd, param.T_max); end输出的图像里Q估计1.bmp和w估计.bmp是滤波器输出与真实值的对比dwd.bmp是角速度估计误差dQd.bmp和dQm.bmp分别是估计四元数误差和量测残差。看到dQm.bmp的高斯白噪声特性基本可以判断量测模型没问题dwd.bmp如果出现缓慢漂移优先检查陀螺噪声的R矩阵是否和实际模型一致。表4.1 结果曲线对应的定位文件名内容判定标准Q估计1.bmp估计四元数与真值对比稳态重合w量测.bmp带噪角速度量测波动范围符合R_wdwm.bmp角速度量测残差白噪声均值为0Tw.bmp控制力矩时间序列未持续饱和5. 把EKF调到稳定的三个验证技巧系统跑通之后离可靠还有一段路。下面这个技巧是我拆这个工程后最想分享的用蒙特卡洛仿真加3σ包络检验判断Q/R矩阵是否调对。把所有随机种子抽出来跑50次同一任务统计每一维估计误差与滤波器协方差标准差之间的归一化比率function ok monte_carlo_3sigma(N, seed) rng(seed); n 13; sigma_ratio zeros(n, N); for i 1:N [x_est, P, x_true] run_single_sim(); % 单次仿真 err x_true - x_est; sig sqrt(diag(P)); sigma_ratio(:, i) abs(err) ./ sig; end % 每维误差超过3σ的比例 over_ratio sum(sigma_ratio 3, 2) / N; % 1表示通过0表示某维超限 ok all(over_ratio 0.05); end如果某维度的over_ratio明显高于5%说明该通道的Q或R设置失配。举例来说角速度维度超限了调大Q_w姿态四元数维度超限调大Q_q同时检查四元数量测噪声R_q是否写小了。这里有一个容易踩的坑P矩阵是滤波器估计的协方差不是真实误差的均方值所以必须通过多次仿真统计后才能使用3σ判据单次仿真看不出来。如果要用stability.m做系统稳定性分析记住它生成的是线性化系统在平衡点的特征值分布只能帮助判断控制器增益是否安全不能验证EKF收敛性。EKF的收敛性验证还得靠蒙特卡洛包络。经过这一轮检验后再回去调PD/LQR参数就能把估计误差和控制精度分开审视不会出现“控制器增益改大反而精度下降”的错觉。本文还有配套的精品资源点击获取