企业官网建设流程全解析

最近在做一个带明显非线性特性的被控对象仿真，需要用状态估计做闭环反馈控制，而且传感器的输出也不是简单的线性关系。刚开始图省事，直接套了线性卡尔曼滤波（KF）跑仿真，结果没几拍就开始发散，误差越滚越大，完全没法用。后来换成扩展卡尔曼滤波（EKF），折腾半天雅可比矩阵的求导，模型一变化又得重新推，效率低还容易算错。最后认真把无迹卡尔曼滤波（UKF）搬进 Simulink，配合闭环控制一起联调，实测下来的效果比前两种方案稳太多。这篇内容就是我这次搭建模型的完整记录，包括算法选型思路、Simulink 的模块结构、参数调优过程以及调试时踩过的几个坑，供正打算做类似估计与控制仿真的朋友参考。

1. 为什么选无迹卡尔曼滤波：从 KF 到 UKF 的选型思考

1.1 线性 KF 的局限与非线性场景

经典卡尔曼滤波处理的是线性高斯系统，它的整套推导都建立在线性状态方程和线性量测方程之上。如果对象是线性常系数系统，传感器也是线性输出，那 KF 就是最优的，没必要上其他滤波算法。但工程里真正的线性系统少之又少，非线性才是常态：弹簧刚度不是常数、阻尼随速度变化、传感器输出带饱和或高次项，这些都会让线性 KF 的假设不成立。

我这次要仿真的对象就是一个非线性弹簧-质量系统，恢复力和位移之间不是简单的比例关系，而是带三次项。传感器输出的也不是精确的位移，而是经过一个非线性变换后的值。面对这种场景，如果强行按 KF 用，状态方程里的矩阵形式就写不出来，系统矩阵 A、量测矩阵 H 都是时变的，KF 的核心公式直接推导不下去。就算某些工况下能在工作点附近做线性化近似，一旦对象偏离工作点，滤波精度马上掉下来，严重的直接发散。

1.2 UKF 与 EKF 的差别，为何在 Simulink 里更合适

EKF 的思路很直接：把非线性函数在当前估计点附近做泰勒展开，保留一阶项，用线性化后的方程替代原来的非线性方程，然后套用 KF 的公式。这个方法听起来合理，但工程实现上有几个麻烦。第一，雅可比矩阵要求导，状态维数一高，手推或者符号推导都很费劲；第二，一阶截断还会引入截断误差，非线性的强度越大这个误差越不可忽略；第三，Simulink 里如果模型参数是变化的，每个步长都要重算雅可比，既麻烦又影响实时性。

UKF 完全是另一种思路，它不需要对非线性函数求导，而是用一组确定性选取的 sigma 点来逼近随机量的分布。这组点经过非线性函数传播之后，再加权求均值协方差，精度上可以达到三阶（对均值而言，协方差二阶），明显优于 EKF 的一阶截断。更关键的是，UKF 的整个流程在 Simulink 里相当好落地，因为它的核心就是“非线性函数直接传播 sigma 点”，完全可以用一个 MATLAB Function 块完成，模型参数变化时也不需要额外推导雅可比，改起来非常灵活。

从选型角度看，如果只是很小的非线性，EKF 也许够用，但一旦非线性强度不能忽略，UKF 的计算量增加并不多。尤其是带反馈控制器的仿真模型里，信号在多个模块之间来回迭代，UKF 的稳定性明显更有保障。我这次最终选它就是基于这个判断。

2. 算法基础与工程化整理：动手前先理清的公式和模块边界

2.1 无迹变换（UT 变换）的核心逻辑

无迹变换是整个 UKF 的地基。它的目标非常简单：已知一个随机向量 x 的均值 x_mean 和协方差 P，要估计经过非线性函数 g(x) 之后的 y 的分布。UT 不直接对 g 做任何近似，而是选取一组代表性的采样点，让它们的均值和协方差刚好等于 x_mean 和 P，再让这些点分别通过 g，最后对得到的输出做加权统计。

对于 n 维状态向量，标准做法是取 2n+1 个 sigma 点。选点公式是：

• 第 0 个点：x0 = x_mean
• 前 n 个点：x_mean + sqrt((n+lambda) * P) 的每一列
• 后 n 个点：x_mean - sqrt((n+lambda) * P) 的每一列

这里的 lambda 是一个尺度参数，定义为 lambda = alpha^2 * (n + kappa) - n。alpha 控制 sigma 点离均值的距离，一般取一个较小的正数比如 1e-3 到 1 之间；kappa 是次级尺度参数，通常设为 0；beta 和高斯分布的先验知识有关，高斯分布下取 2 比较合适。

权重的计算是：

• 权重Wm0 = lambda / (n + lambda)
• 权重Wc0 = lambda / (n + lambda) + (1 - alpha^2 + beta)
• 第 i 个点的权重Wm_i = 1 / (2 * (n + lambda))，Wc_i 和 Wm_i 相同

这里 Wm 用于算均值，Wc 用于算协方差。第 0 个点的 Wm 和 Wc 不相等，多出来的部分就是 beta 的修正项，用于补偿高斯分布的高阶信息。

2.2 预测与更新两个阶段的标准方程

UKF 的递推结构和 KF 一样，也是“预测-更新”两段式，区别在于每一步都通过 sigma 点来传递分布。

预测阶段，先根据上一拍的后验状态生成 sigma 点，然后让每个点经过状态方程 f，得到传播后的点集。加权求均值得到先验状态 x_apri，加权求协方差再加上过程噪声 Q，得到先验协方差 P_apri。这一步本质上就是用一个确定的点集代替蒙特卡洛的大量随机采样。

更新阶段，用先验状态重新生成一组 sigma 点，让这些点经过量测方程 h，得到量测估计的均值 z_mean 和量测协方差 Pzz，同时计算状态和量测的互协方差 Pxz。增益矩阵 K = Pxz / Pzz（即 Pxz 乘以 Pzz 的逆）。后验状态 x_post = x_apri + K * (z - z_mean)，后验协方差 P_post = P_apri - K * Pzz * K^T。

工程实现里有一点很容易被忽略：在更新阶段，Pzz 矩阵的维数是量测维数 m 乘 m，只有当量测更新维度大于 1 时才是个矩阵。m=1 时它就是标量，直接取倒数就行，没必要调用矩阵求逆函数，省一点计算开销。

2.3 Simulink 中组织 UKF 代码的推荐模块划分

在 Simulink 里搭 UKF，不只是把算法公式写成 MATLAB 代码，还要考虑仿真结构怎么组织。最常见的错误做法是把整个 UKF 写成一个巨大的 function，输入输出错综复杂，查错的时候根本分不清是哪一步出了问题。

我建议把模型拆成四个清晰的模块：

• 被控对象模块：接收控制量 u 和过程噪声，输出系统真实状态，再经过传感器模型输出量测值 z。
• UKF 估计器模块：输入上一拍后验状态、上一拍后验协方差、当前控制量、当前量测值，输出当前拍后验状态和协方差。
• 反馈控制器模块：接收估计状态，按照设计好的反馈增益生成控制量。
• 数据显示与记录模块：把真值、估计值、误差、控制量送到示波器或者写入工作空间。

UKF 估计器内部如果再细分，可以分成 sigma 点生成、状态传播、量测更新三个部分，但在代码实现上一般不拆成三个独立模块，而是在同一个 MATLAB Function 块里分步骤写。拆太散会引入不必要的信号连线，导致仿真模型变得臃肿。保持一个函数块内部结构清晰即可。

3. 仿真模型搭建实操：一个非线性弹簧阻尼对象的完整闭环案例

3.1 被控对象建模与参数设定

为了让接下来的步骤有针对性，我用一个具体对象说明：一个带非线性弹簧和线性阻尼的质量块，控制量为外力 u，传感器测量值经过非线性变换输出。

状态变量取两个：x1 为质量块位移，x2 为质量块速度。连续时间状态方程写出来是这样：

• x1_dot = x2
• x2_dot = (-b/m) * x2 - (k0/m) * x1 - (k1/m) * x1^3 + u/m

m 是质量，b 是阻尼系数，k0 是线性刚度系数，k1 是三次非线性刚度系数。这里 x1^3 引入了非线性，使得系统状态转移矩阵不能写成常系数形式。

量测方程也不是简单的线性输出，传感器给出的信号是：

• z = 2 * x1 + 0.3 * sin(x1) + v

其中 v 是量测噪声。这个 sin 项就是典型的非线性传感器特征。

仿真计算采用固定步长离散化，步长 dt 取 0.01 秒。离散后的状态方程写成递推形式：

• x1(k+1) = x1(k) + x2(k) * dt
• x2(k+1) = x2(k) + (-b/m * x2(k) - k0/m * x1(k) - k1/m * x1(k)^3 + u(k)) * dt

我这次用的参数值为 m=1 kg，b=0.5 N·s/m，k0=2 N/m，k1=0.8 N/m³。这些数值本身不重要，关键是让非线性项在仿真动态范围内有可见影响，不会因为非线性太弱而看不出效果。

3.2 Simulink 顶层结构与信号连接方式

顶层模型从输入到输出形成这样的闭环链：控制器产生 u，送给被控对象模型；被控对象输出真实状态和量测 z；量测 z 连同 u 一起送给 UKF 估计器；估计器输出 x_hat 和 P，其中 x_hat 反馈给控制器做状态反馈；P 送到 Unit Delay 模块存到下一拍使用。

这里有一个很关键的工程细节：UKF 估计器内部需要上一拍的后验状态和协方差，所以 x_hat 输出端必须接一个 Unit Delay，延迟一拍以后再作为估计器的输入。P 矩阵在 Simulink 里作为 2x2 矩阵信号传递，如果直接用矩阵信号接 Unit Delay，初始化时需要把初值设成 2x2 的矩阵常量。我在实际搭模型时习惯把 P 矩阵通过 reshape 展成一维向量，再接 Unit Delay，这样初值设置更直观，也不容易遇到维数不匹配的问题。初始协方差 P0 我取 diag([0.1; 0.1])，初始状态 x0 取 [0; 0]。

被控对象模块内部用 MATLAB Function 块实现离散状态方程，输出真值 x_true 和量测 z。过程噪声通过 Simulink 自带的噪声发生器模块生成，加到状态更新方程上。量测噪声也在被控对象模块内叠加到 z 上，这样最符合真实传感器的情况：噪声是物理过程产生的，对估计器来说是未知的。

3.3 UKF 估计器核心代码示例

在 Simulink 的 MATLAB Function 块里，函数头包括输入输出定义，具体代码可以这样组织：

function [x_hat_out, P_out] = UKF_Step(x_hat_in, P_in, u_in, z_in) %#codegen % 固定参数 dt = 0.01; m = 1.0; b = 0.5; k0 = 2.0; k1 = 0.8; % 噪声协方差 Q = diag([0.01, 0.01]); R = 0.05; % UKF 参数 n = 2; % 状态维数 alpha = 0.01; beta = 2; kappa = 0; lambda = alpha^2 * (n + kappa) - n; % 权重 Wm = zeros(2*n+1, 1); Wc = zeros(2*n+1, 1); Wm(1) = lambda / (n + lambda); Wc(1) = lambda / (n + lambda) + (1 - alpha^2 + beta); for i = 2 : 2*n+1 Wm(i) = 1 / (2 * (n + lambda)); Wc(i) = 1 / (2 * (n + lambda)); end % 1. 根据上一拍后验状态生成 sigma 点 P_sqrt = sqrtm((n + lambda) * P_in); chi = zeros(n, 2*n+1); chi(:, 1) = x_hat_in; for i = 1 : n chi(:, i+1) = x_hat_in + P_sqrt(:, i); chi(:, n+i+1) = x_hat_in - P_sqrt(:, i); end % 2. 状态传播：sigma 点逐个通过状态方程 x_pred = zeros(n, 2*n+1); for i = 1 : 2*n+1 x1 = chi(1, i); x2 = chi(2, i); x_pred(1, i) = x1 + x2 * dt; x_pred(2, i) = x2 + (-b/m * x2 - k0/m * x1 - k1/m * x1^3 + u_in) * dt; end % 3. 先验均值与协方差 x_apri = zeros(n, 1); for i = 1 : 2*n+1 x_apri = x_apri + Wm(i) * x_pred(:, i); end P_apri = Q; for i = 1 : 2*n+1 diff = x_pred(:, i) - x_apri; P_apri = P_apri + Wc(i) * (diff * diff'); end P_apri = (P_apri + P_apri') / 2; % 4. 根据先验状态重新生成 sigma 点，经量测方程传播 P_sqrt2 = sqrtm((n + lambda) * P_apri); chi2 = zeros(n, 2*n+1); chi2(:, 1) = x_apri; for i = 1 : n chi2(:, i+1) = x_apri + P_sqrt2(:, i); chi2(:, n+i+1) = x_apri - P_sqrt2(:, i); end z_pred = zeros(1, 2*n+1); for i = 1 : 2*n+1 z_pred(i) = 2 * chi2(1, i) + 0.3 * sin(chi2(1, i)); end z_mean = 0; for i = 1 : 2*n+1 z_mean = z_mean + Wm(i) * z_pred(i); end Pzz = R; Pxz = zeros(n, 1); for i = 1 : 2*n+1 dz = z_pred(i) - z_mean; dx = chi2(:, i) - x_apri; Pzz = Pzz + Wc(i) * dz * dz'; Pxz = Pxz + Wc(i) * dx * dz; end % 5. 增益更新与后验计算 K = Pxz / Pzz; innov = z_in - z_mean; x_hat_out = x_apri + K * innov; P_out = P_apri - K * Pzz * K'; P_out = (P_out + P_out') / 2; end

这个函数在模型里每个采样周期调用一次。输入 x_hat_in 和 P_in 来自 Unit Delay 的输出，u_in 来自控制器，z_in 来自被控对象的量测输出。输出 x_hat_out 送到控制器，P_out 送到 Unit Delay。

有一点特别提醒：代码里用 sqrtm 而不是 chol，原因是最初几拍 P 矩阵可能因为数值问题不完全符合正定条件，chol 会因为非正定报错。sqrtm 对对称半正定矩阵的容忍度更高，虽然计算量稍大，但在仿真验证阶段稳定。

3.4 仿真参数设置与初始化要点

Simulink 仿真参数有几个容易被忽略的坑。求解器要选固定步长，离散仿真用 discrete 求解器就行，如果被控对象里有连续模块，用定步长的 ode4（四阶龙格库塔）加固定步长 0.01。步长必须和模型里用的 dt 保持一致，否则采样时间不匹配会造成滤波结果和真实对象之间出现一个步长的偏差，看起来像延迟，但实际是配置错误。

随机噪声模块的参数要特别注意。过程噪声可以用带限白噪声或者高斯噪声发生器生成，但采样时间要设成与模型步长相同，否则生成的噪声序列统计特性不对。噪声协方差的大小要通过模块参数调整到和代码里 Q、R 矩阵的设置匹配，这个在模型里没有一个自动校验机制，只能手动核对。

初始化方面，x0 和 P0 可以放在模型初始化回调里写，或者直接在 workspace 里定义好后在模型参数里引用。我倾向于把 m、b、k0、k1、dt、Q、R、alpha、beta、kappa 这些参数全部放在一个初始化脚本里，每次改参数重新跑一下脚本就行，不用在模型里到处找。

4. 参数调优、仿真对比与结果分析

4.1 过程噪声 Q 与量测噪声 R 的调整方向

UKF 里 Q 和 R 的物理含义和 KF 里完全相同，Q 体现你对过程模型的信任程度，R 体现你对量测的信任程度。Q 取值小意味着认为模型很准，增益就小，滤波输出平滑；Q 取值大意味着认为模型不可靠，更多依赖量测，响应变快但噪声放大。R 的影响正好相反，R 大表示量测信噪比低，滤波偏向相信模型预测；R 小表示量测可信度高，更新步增益大。

实际调试时不要一上来就同时调 Q 和 R，那样很难定位问题。我惯用的顺序是：先把 R 固定在一个与传感器噪声统计匹配的量级上，然后调 Q。如果稳态误差明显，但响应还跟得上，说明 Q 太小，适当增大；如果出现明显的高频抖动，说明 Q 太大，往回压。调完 Q 再看 R 的影响，如果滤波结果偏滞后，适当减小 R，让量测权重加大。

按照这个思路，我给出一组在调试中比较有代表性的参数和结果。传感器噪声标准差约为 0.2，所以 R 量级可以取 0.05 到 0.1 之间。下面表格是不同 Q 取值下的仿真效果。

4.2 两组典型仿真结果与直观对比

我用三组参数做了对比仿真，每组仿真时长 5 秒，统计稳态段的位移估计误差。

从这组数据可以明显看到，Q 太小和 Q 太大都有各自的问题。中间那组参数在这个对象上表现最平衡，既没有明显的滞后，也不会放大噪声。需要说明的是，这里的数值是特定对象和特定仿真条件下的结果，换成别的对象需要重新标定，不能直接抄。

闭合控制回路里的表现也很关键。第三组参数下控制量 u 出现了明显的高频成分，这是因为估计状态噪声大，反馈控制把这些噪声引入了执行器。实际控制系统里这是很危险的事情，执行器会发热、磨损加剧，所以调参时不能只看估计误差，还要一起看控制量的波动。

4.3 结果分析：估计误差来源与收敛性规律

从仿真曲线来看，UKF 的收敛过程有一个特点：前 0.3 秒误差较大，然后快速收窄。这个阶段的误差来源于初始状态 P0 设置和真实初始状态的偏差。P0 如果设置过大，前几拍增益偏高，估计值会先出现一个短暂的过冲；P0 如果设置过小，收敛速度又太慢。比较省事的做法是 P0 取一个中等偏大的值，不要一开始就追求精确，让滤波器自己把误差消化掉。

稳态段的误差主要由三部分构成：过程噪声激励引起的随机波动、量测噪声引起的随机波动、模型失配引起的系统偏差。在这个模型里没有模型失配，所以主要是前两项。UKF 相比 EKF 的优势在非线性的幅值比较大的时候更加明显，线性化误差小了，系统偏差也就小了。

我在这个对象上专门测过一个极端场景：把初始估计位置故意设到真值相反的方向，也就是 x_hat0 = [-2.0; 0]，P0 保持 diag([0.1; 0.1])。UKF 仍然能在 0.5 秒左右把状态拉回真值附近，而 EKF 在同样的条件下出现了两次发散。这说明 UKF 对大误差初始条件的鲁棒性确实强一些，和理论分析一致。

5. 常见问题与排查技巧实录

5.1 滤波发散，估计误差越来越大

这是 UKF 仿真里最常见也最让人头疼的问题。现象就是估计值逐渐偏离真值，之后完全回不来。我遇到过的原因一般有四种。

第一，Q 矩阵和 R 矩阵严重不匹配。比如 R 设得非常小，量测噪声又实际很大，滤波器就会完全相信量测，把大量噪声当成有效信息，最后方差被噪声淹没。判断办法是看输出曲线是否出现高频毛刺，如果是，先把 R 调大一个数量级试试。

第二，离散步长过大导致状态方程传播误差太大。UKF 的 sigma 点虽然能处理非线性函数，但它是用离散方程传递的，如果 dt 太大，离散化本身的截断误差会体现在预测协方差里。此时即使 Q 设得足够大也救不回来，因为问题出在模型本身。解决办法是把 dt 缩小，验证曲线是否明显改善。

第三，P 矩阵失去正定性。这通常发生在长时间运行之后，由于数值舍入误差，P 的对称性被破坏，导致 sqrtm 或后续协方差更新出现非物理值。我在代码里已经加了对称化处理，但更彻底的方案是在每个步长末尾检查 P 的特征值，如果出现负特征值就做一次修正，将负特征值截断为零。

第四，初始 P0 设置不当。P0 如果比实际误差小几个数量级，前几拍增益太小，状态收敛特别慢，看起来也像发散。检查方法是观察前 10 拍的估计误差有没有明显收缩趋势，如果没有，把 P0 扩大 10 倍重试。

5.2 量测更新里的角度跳变问题

如果你的量测方程里有方位角，那么会遇到一类特殊的跳变问题：atan2 的输出范围是 -pi 到 pi，当真实角度跨过边界时，量测值会突然从接近 pi 跳到接近 -pi，或者反过来。对应到 UKF 里，创新项也就是 z 减去 z_mean 会出现一个接近 2pi 的跳变，导致增益计算打出巨大的修正，估计状态被强行拉偏。

这类问题的标准解法是对创新项的每个角度分量做归一化处理。判断角度差，如果大于 pi 就减去 2pi，如果小于 -pi 就加上 2pi，把差值限制在 -pi 到 pi 区间内再参与增益计算。更保险的做法是不要直接对角度本身做加权平均，而是把角度转换成 sin 和 cos 分量分别处理，但那样会改变量测方程的维度结构，实现起来更复杂。对于一般仿真，做创新项归一化就够了。

5.3 模型运行报错：信号维度不匹配与代数环

Simulink 工程问题里最常见的其实不是算法问题，而是连接问题。UKF 函数的输出 P_out 是 2x2 矩阵，Unit Delay 的输出也是矩阵，但如果中间经过了某些只支持向量信号的模块，就会报维度错误。解决办法是把 P 矩阵通过 reshape 模块展成 4 维向量，经过处理后再拼回矩阵形状。

代数环出现的典型位置是：控制器输出 u，ukf 模块需要 u 作为输入，但 ukf 输出 x_hat 又去计算 u，这就形成一个没有延时的闭环。Simulink 会报代数环错误，或者在每个步长内做迭代求解，导致仿真速度骤降。这个问题的根治办法是给 x_hat 或者 P_out 加 Unit Delay，让反馈路径上至少存在一个采样周期的延迟。从实际控制系统角度看，这个延迟通常是合理的，因为后验状态本来就是要到下一个周期才能用于控制。

5.4 代码生成与实时性相关的问题

如果后续打算把模型生成 C 代码部署到嵌入式设备上，UKF 的实现里有两个点提前要考虑。第一，sqrtm 在代码生成环境里支持不理想，建议换成 Cholesky 分解，而为了保证 Cholesky 稳定，需要在每次更新后强制 P 对称正定，必要时给对角元加一个极小的正值。第二，MATLAB Function 块里尽量不要用动态变大小数组，sigma 点矩阵的维度在编译期必须固定，我上面的代码把 2n+1 直接写成了 5，就是考虑到代码生成的限制。

关于实时性，UKF 的计算量主要集中在 sigma 点传播上，状态维数为 n 时每次要跑 2n+1 次非线性函数。如果状态维数很高，计算负担会明显上升。对于实时控制，可以考虑减少 sigma 点的数量或者改用简化形式的 UKF，但这是另一个话题，仿真阶段不必过度优化。

6. 一些实操中的体会与后续扩展方向

我在多次调试这类估计器模型后，最大的体会是：UKF 的上手成本比 EKF 低得多，但前提是你先把对象的非线性项列清楚。很多人拿到一个模型就直接往滤波器里塞，连状态方程是连续还是离散都没确认，导致仿真结果不对还找不到原因。我的习惯是先用一个开环仿真把对象的动态特性摸清楚，再看状态变量的量级，最后才定噪声协方差的初值。Q 和 R 不是拍脑袋出来的，它们要和真实物理系统的扰动水平对应。

另外一个小技巧是在模型里把真值和估计值同时接进示波器。不要只盯着估计曲线看，一定要和真值对比着看。如果两者完全重合也未必是好事，要怀疑是不是把真值不小心也传进了滤波器，相当于作弊。这个错误我在最初搭模型时犯过一次，排查了好久才发现是连线问题。

这个模型还可以继续扩展的方向不少。比如把量测改成两个传感器融合，加一个非线性测距传感器，用扩展形式的量测向量做 UKF，可以顺便验证多源融合对估计精度的提升。或者在控制回路里加一个参考信号跟踪，把 UKF 估计的状态用在 LQR 或者滑模控制器上，看看不同控制器对估计误差的敏感程度。如果对象再复杂一点，状态维数上去了，还可以对比标准 UKF 和平方根 UKF 在数值稳定性上的差别。这些都是很好的后续练习题，在这个基础上改模型比从零开始搭要省力得多。