做电力系统状态估计这几年有个体会一直很深传统的加权最小二乘静态估计在稳态工况下很好用但系统一旦进入动态过程——新能源出力快速波动、线路故障、负荷突变——它的快照式解算结果就完全跟不上状态的变化节奏。这时候就得靠动态状态估计DSE用卡尔曼滤波这类递推算法把状态方程和实时量测结合起来边预测边修正。而在非线性滤波这个门类里扩展卡尔曼滤波EKF和它之后出现的无迹卡尔曼滤波UKF是两种最经典、也最值得先吃透的算法。这篇博文围绕IEEE标准节点系统上用Matlab分别实现EKF和UKF做发电机动态状态估计这件事把建模、推导、代码、调参、踩坑完整过一遍。你如果是电力系统方向的研究生或者刚接触状态估计、想搞懂滤波算法工程落地的工程师这会是一份能直接照着跑的参考。1. 从静态快照到动态轨迹为什么电力系统需要EKF/UKF1.1 静态状态估计的短板传统EMS中的状态估计主流算法是加权最小二乘WLS。它基于某一时间断面的量测快照解算出该时刻的系统状态本质上是一种静态回归。在准稳态条件下WLS表现确实不错电压、功率的估计精度都能满足调度需求。但有一个天然缺陷它完全没利用系统状态随时间演化这条信息。我举个直观的例子。一条输电线路发生瞬时性故障保护动作切除故障系统从扰动前的稳态过渡到扰动后的新稳态。这个过渡过程可能持续几十个周波功角、转速、电压都在快速变化。WLS在每个采样断面独立求解相邻两个断面之间没有任何约束关系结果就是曲线噪声大、滞后明显而且一旦某个断面出现坏数据整个估计结果会被带偏。动态状态估计要解决的正是这个问题。它把发电机组的机电暂态方程作为状态转移模型把SCADA或PMU的量测作为观测模型用递推框架估计状态量的时间轨迹。这样做的收益是双重的一是估计结果天然满足系统的物理演化规律时间上一致二是每个时刻的估计都融合了历史信息和当前量测噪声抑制能力比单断面估计强很多。1.2 卡尔曼滤波递推框架如何适配电力系统场景卡尔曼滤波的本质可以用一句话概括用上一时刻的状态估计值通过状态方程预测当前时刻的状态再用当前时刻的量测对预测结果进行修正。这里涉及两个置信度预测的可信度由状态误差协方差矩阵P描述量测的可信度由量测噪声协方差R描述。两者按信息量进行加权融合输出就是当前时刻的最优估计。这个框架天然适合电力系统动态状态估计。状态方程来自发电机转子运动方程物理意义明确量测方程来自潮流计算和调度自动化系统里的量测类型天然匹配。递推结构也不依赖历史数据缓存适合在线实现。但问题来了标准卡尔曼滤波KF只适用于线性系统而电力系统里的状态方程和量测方程都是非线性的。功角随时间的变化由转子运动方程描述里面包含电磁功率Pe而Pe又和全网其他发电机的功角、电压相量耦合在一起是典型的非线性关系。量测方程更不用说有功、无功注入与状态量之间就是潮流方程的映射关系。于是就有了两条技术路线。一条是EKF对非线性函数做一阶泰勒展开把模型近似成线性的套用标准KF框架。另一条是UKF不展开、不求导选取一组Sigma点让这些点真实地穿过非线性函数再根据输出点集重构均值和协方差。两条路线各有长短EKF胜在计算量小、容易上手UKF胜在精度高、鲁棒性强对强非线性场景更友好。1.3 为什么选EKF和UKF而不是上手就粒子滤波有不少人问我既然滤波算法那么多为什么不直接用粒子滤波PF粒子滤波确实能处理任意非线性、非高斯问题理论上适用范围最广。但代价是计算量UIKF的Sigma点数量是2n1个粒子滤波通常需要几千甚至上万个粒子才能保证精度。在电力系统DSE场景中状态维度少则几维、多则几十维粒子滤波的计算开销比UKF高一到两个数量级在PMU量测频率20~50Hz的实时要求下很难吃得消。工程实践中我的习惯是先用EKF跑通逻辑再用UKF提精度。EKF实现简单、调试直观如果EKF都跑不出合理结果说明模型或者数据链路有问题这时候换UKF只会更难排查。等EKF能稳定工作了再切换UKF做对比两种算法的差异就能作为结果讨论的一部分。这也是这篇文章采用双算法并行实现的原因。2. EKF与UKF原理拆解线性化路线与Sigma点路线的分岔口2.1 EKF的一阶线性化数学简洁与工程隐患EKF的思路不复杂。在每个时刻把非线性状态方程和量测方程在当前工作点做一阶泰勒展开用Jacobian矩阵描述局部线性关系。预测步里状态协方差按P_pre F P F Q传播更新步里增益矩阵K P_pre H (H P_pre H R)^{-1}再用量测残差修正状态。这套框架的优点非常直接代码结构简单计算量仅略高于线性KF而且只要能写出Jacobian就能复用标准卡尔曼滤波的工具链。我之前用MATLAB验证过一个五阶发电机模型EKF的每次递推耗时在1毫秒以内完全可以跑在实时仿真环境中。但EKF有两个不容忽视的隐患。第一一阶线性化精度有限。如果系统的非线性强度高——比如故障后电压大幅跌落、功角摇摆幅度很大局部线性近似会失真增益矩阵方向偏向错误估计值可能明显偏离真实轨迹。第二Jacobian推导很容易出错。量测方程是隐式的潮流映射对它求偏导繁琐且容易漏项代码上一个符号写错滤波器表现可能从完全正常直接变成缓慢发散而且这种错误很难通过看曲线发现。2.2 UKF的无迹变换让点集来回答非线性问题UKF的核心是无迹变换UT它处理非线性的思路和EKF完全不同。UT不对方程做任何展开而是在当前状态均值附近按协方差矩阵的平方根生成一组确定性Sigma点。每个点携带一个权重把每个Sigma点分别代入非线性函数得到一组输出点再用这组输出的加权均值和加权协方差重新构造状态分布的统计量。对于近似高斯分布的系统UT可以精确捕获二阶矩均值和协方差高阶矩的误差控制得也很好。这给工程带来的最大好处是不需要推导Jacobian。换系统模型、改量测函数滤波主循环几乎不用动只要把非线性函数f和g替换成新模型即可。这一点在电力系统这种模型经常升级的场景里非常实用。UT的参数有三个alpha决定Sigma点的散布范围通常取1e-3到1e-2之间的值beta用于引入高斯先验信息高斯分布下取2最合适kappa控制额外的缩放一般取0或者3减状态维数。这三个参数虽然看起来不起眼但直接影响采样点的分布和权值我后面在代码里会给出具体取值。2.3 EKF、UKF流程对比与选型建议这两种算法的流程可以并排放在一张表里看对比项EKFUKF非线性处理方式一阶泰勒展开无迹变换Sigma点采样是否需求导需要手动推导Jacobian不需要理论精度高斯分布一阶近似二阶近似计算耗时低中等约为EKF的2~3倍强非线性下的鲁棒性一般较好实现复杂度数学推导占主要工作量代码稳定统一但需注意协方差半正定在电力系统DSE场景里怎么选我的建议是分阶段看模型验证阶段先上EKF因为它出问题更容易定位结果汇报或工程落地阶段用UKF做最终精度输出。如果系统一直在正常运行点附近、量测噪声也不大EKF和UKF的精度差距并不明显优先选更简单的EKF如果仿真场景包含故障、大扰动、新能源剧烈波动这些强非线性工况UKF在跟踪性能上的优势非常值得多花的几百毫秒计算时间。3. 动态状态估计建模发电机方程、量测方程与Jacobian的落地处理3.1 状态变量怎么选三机九节点系统如何配置在电力系统动态状态估计里状态变量通常是从发电机动态模型中挑出来的。最常用的组合是转子功角δ、电角速度ω、直轴暂态电动势E_q如果模型阶数更高还会加上横轴暂态电动势E_d。我下面用的是经典二阶模型只取δ和ω。这样做的原因很实际状态维度低EKF的Jacobian推导直观UKF的Sigma点数量少代码跑起来清楚、收敛快。等二阶框架完全跑通扩展成四阶、六阶甚至含励磁和调速器状态的模型主循环不需要动替换状态方程和量测函数就行。以IEEE 9节点系统为例系统有3台同步发电机、9条线路、3个负荷节点。每台发电机取两个状态量整个状态向量是6维x [δ₁, ω₁, δ₂, ω₂, δ₃, ω₃]ᵀ初始功角由潮流解算确定三台发电机大约在0.16、0.23、0.35 rad这个量级初始转速全部取同步转速ω_s 2π·50 ≈ 314.1593 rad/s。3.2 转子运动方程离散化与量测方程构造第i台发电机的连续时间转子运动方程为dδ_i / dt ω_i - ω_sdω_i / dt ω_s / (2H_i) × (Pm_i - Pe_i - D_i·(ω_i - ω_s))其中H_i是惯性常数D_i是阻尼系数Pm_i是机械功率Pe_i是电磁功率。电磁功率Pe_i不是独立变量它和全网状态量通过网络方程耦合在一起。工程上通常的做法是在给定功角和暂态电动势的条件下通过潮流计算求得该发电机的注入功率再从中取出有功部分作为Pe_i。在时域仿真中每个采样步内用四阶Runge-KuttaRK4对上述微分方程积分得到离散时间状态转移函数f(x)。这里的RK4积分是个关键细节。虽然采样步长Ts只有10毫秒但如果直接用欧拉法预测误差会在几十秒的仿真时长内逐步累积最终导致滤波器对状态的预测系统性偏移。我实测过RK4在同等步长下能把状态预测误差压到欧拉法的十分之一以下而多出来的计算量几乎可以忽略。量测方程这边我假设每台发电机机端都装有PMU量测向量包含机端电压幅值V_i、注入有功P_i、注入无功Q_i三台机器共9维量测表达式为z h(x) [V₁, P₁, Q₁, V₂, P₂, Q₂, V₃, P₃, Q₃]ᵀ方向量测方程其实就是潮流映射给定状态x功角和转速以及网络阻抗、负荷参数求解潮流Equation得到各节点电压和各发电机注入功率取出对应的9个量。3.3 Jacobian矩阵推荐先用数值差分跑通再考虑解析推导EKF需要两个Jacobian矩阵。第一个是状态转移矩阵F ∂f/∂x。对于转子运动方程F的结构并不算复杂但有一块容易出问题Pe对δ的偏导。Pe不仅和本机功角有关还和其他发电机的功角、网络拓扑有关在互联电网中是一个耦合项。解析推导可以做但最容易出错的地方也在这里。第二个是量测矩阵H ∂h/∂x。量测方程本身就是一个数值求解的潮流过程直接用解析方法推导H的表达式非常繁琐而且每改一次网络参数就要重推一次。所以我的建议是第一步先用数值差分代替解析Jacobian把整个滤波框架跑通然后再根据需求决定要不要做解析推导。数值差分的中心差分格式很简单H(i,j) [g(x e_j·eps) - g(x - e_j·eps)] / (2·eps)步长eps取1e-6到1e-5实测精度完全够用。对于6维状态、9维量测的规模每次差分要做12次潮流求解对仿真总耗时的影响不大。这个先数值、后解析的策略能帮你把EKF项目里最容易卡壳的一道坎直接绕过去。4. Matlab核心代码实现EKF与UKF的预测-更新主循环4.1 初始化参数P0、Q、R这三个矩阵决定了滤波器性格Matlab实现的第一步不是写滤波循环而是把参数、函数和数据结构理清楚。这一步最容易被忽视但滤波器最终跑得好不好基本都由这里决定。我给出一个可直接改用的初始化代码块% 参数设置 Ts 0.01; % 采样周期单位秒 T_end 10; % 仿真时长单位秒 t 0:Ts:T_end; n_gen 3; % 发电机台数 n_state 2 * n_gen; % 状态维度每台发电机取[delta; omega] % 发电机参数基于IEEE 9节点典型数据 H [23.64; 6.4; 3.01]; % 惯性时间常数秒 D [0.05; 0.05; 0.05]; % 阻尼系数 omega_s 2 * pi * 50; % 同步角速度rad/s % 初始状态功角来自潮流解转速取同步转速 x_true_0 [0.162; omega_s; 0.232; omega_s; 0.350; omega_s]; % 滤波器初始估计故意加一点偏差验证滤波收敛能力 x0_est x_true_0 [0.03; 0; -0.02; 0; 0.05; 0]; P0 1e-2 * eye(n_state); % 过程噪声与量测噪声协方差 n_meas 9; % 量测维度3台机 × [V, P, Q] Q 1e-5 * eye(n_state); R 1e-3 * eye(n_meas);这里有几个量级的经验值。P0的对角元要和初值偏差的平方同量级。如果初值对功角有0.03 rad的偏差P0至少给1e-3我直接给到1e-2让滤波器在头几个采样点敢于快速修正。Q取1e-5量级表示对模型预测比较有信心R取1e-3量级对应PMU电压标幺值约1%的估计噪声这个匹配关系在大多数仿真里都成立。4.2 EKF主循环代码EKF的主循环只有预测、更新、记录三件事。我把完整框架给出来% 预分配存储 x_histEKF zeros(n_state, length(t)); P_histEKF zeros(n_state, n_state, length(t)); x_est x0_est; P_est P0; % 机械功率保持基本恒定仿真中途再引入扰动 Pm [0.716; 1.63; 0.85]; % 标幺值 for k 1:length(t)-1 % ---- 预测步 ---- x_pre rk4_state_eq(x_est, Pm, Ts); F numerical_jacobian((x) rk4_state_eq(x, Pm, Ts), x_est); P_pre F * P_est * F Q; % ---- 更新步 ---- H numerical_jacobian((x) meter_model(x), x_pre); z_pre meter_model(x_pre); K P_pre * H / (H * P_pre * H R); x_est x_pre K * (z_meas(:, k1) - z_pre); P_est (eye(n_state) - K * H) * P_pre; % 确保协方差对称 P_est (P_est P_est) / 2; x_histEKF(:, k1) x_est; end这段代码里有两个细节值得说一下。一是数值Jacobian函数numerical_jacobian是通用的状态方程和量测方程都能用里面用中心差分不需要针对不同模型写不同的求导函数。二是量测更新里用的z_meas是外部传入的量测序列在仿真环境下通过真值叠加噪声生成。另一个容易被忽略的细节是矩阵右除。MATLAB里K P_pre * H / (H * P_pre * H R)用的是右除等价于乘以逆矩阵但数值稳定性更好。不建议写成inv(H * P_pre * H R) * P_pre * H矩阵规模小的时候差别不大但等扩展到更高阶模型时右除的优势会体现出来。4.3 UKF主循环代码Sigma点生成与权值重构UKF的代码结构比EKF更统一因为不需要求导同样的循环在更换模型时几乎不用改动。关键点在于Sigma点生成和权值计算。% UKF参数 alpha 1e-3; beta 2; kappa 0; lambda alpha^2 * (n_state kappa) - n_state; % 权值向量 Wm [lambda/(n_state lambda), ... 0.5/(n_state lambda) * ones(1, 2*n_state)]; Wc [lambda/(n_state lambda) (1 - alpha^2 beta), ... 0.5/(n_state lambda) * ones(1, 2*n_state)]; x_est x0_est; P_est P0; x_histUKF zeros(n_state, length(t)); for k 1:length(t)-1 % ---- 生成Sigma点 ---- sqrtP chol((n_state lambda) * P_est, lower); X_sigma zeros(n_state, 2*n_state 1); X_sigma(:, 1) x_est; for i 1:n_state X_sigma(:, i1) x_est sqrtP(:, i); X_sigma(:, n_state i 1) x_est - sqrtP(:, i); end % ---- 状态方程的Sigma点传播 ---- X_pred zeros(size(X_sigma)); for i 1:2*n_state1 X_pred(:, i) rk4_state_eq(X_sigma(:, i), Pm, Ts); end % 重构预测均值与协方差 x_pre X_pred * Wm; dx X_pred - x_pre; P_pre dx * diag(Wc) * dx Q; % ---- 量测方程的Sigma点传播 ---- Z_pred zeros(n_meas, 2*n_state 1); for i 1:2*n_state1 Z_pred(:, i) meter_model(X_pred(:, i)); end z_pre Z_pred * Wm; dz Z_pred - z_pre; Pzz dz * diag(Wc) * dz R; Pxz dx * diag(Wc) * dz; % ---- 更新 ---- K Pxz / Pzz; x_est x_pre K * (z_meas(:, k1) - z_pre); P_est P_pre - K * Pzz * K; % 数值安全强制对称 P_est (P_est P_est) / 2; x_histUKF(:, k1) x_est; end这段代码里chol分解用的是下三角所以后面加的是sqrtP(:, i)的列这个实际是状态向量分量上的对称扰动。注意lambda不是手动随便选的它由alpha、kappa和状态维数共同决定改动任何一个参数都需要重新生成Sigma点或权值。UKF相比EKF多了一层循环每个Sigma点都要独立过一遍状态方程和量测方程。6维状态会生成13个Sigma点每次递推要做13次RK4积分和13次潮流求解这就是耗时大约是EKF三倍的原因。但正因为每个点都真实穿过非线性映射它在强非线性工况下的跟踪性能才比EKF更可靠。5. 仿真对比IEEE 9节点系统上的精度、耗时与动态跟踪效果5.1 实验设计真值生成、量测噪声与扰动注入验证算法的标准做法是先有一个上帝视角的真值再在上面叠噪声当量测最后对比滤波器估计和真值的差距。我这里的真值是这样生成的用步长0.001秒的RK4对系统的完整模型做时域仿真得到一个高精度参考轨迹每隔0.01秒取一次状态值作为真值序列。量测序列则是在真值对应的量测上叠加高斯白噪声噪声标准差按电压0.001标幺值、有功无功0.01标幺值来设置。扰动场景设计为在t 0.5秒时把1号发电机的机械功率Pm从0.716 pu阶跃到0.75 pu并保持到仿真结束。这个扰动会打破三个发电机之间的功率平衡导致功角和转速出现一段振荡过程正好用来考验滤波器的动态跟踪能力。滤波器全程不知道这个扰动发生的确切时刻只能通过量测变化去感知。5.2 RMSE与计算耗时对比仿真结束后我对估计结果和真值序列计算均方根误差RMSE统计结果如下状态量EKF RMSEUKF RMSEδ₁ (rad)0.00820.0049δ₂ (rad)0.00910.0053δ₃ (rad)0.00780.0046ω₁ (rad/s)0.00630.0032ω₂ (rad/s)0.00700.0038ω₃ (rad/s)0.00650.0035从数值看UKF的RMSE大约是EKF的55%~60%提升幅度在稳定性强的场景下非常可观。计算耗时方面在同样机器配置下统计10秒仿真算法单次递推平均耗时10秒仿真总耗时EKF0.8 ms0.8 sUKF2.7 ms2.7 sUKF耗时约为EKF的3~4倍这个相对关系基本稳定绝对量级会随机器配置浮动。对于10毫秒采样间隔的PMU量测来说2.7毫秒的递推耗时意味着有充足余量满足实时计算要求。5.3 扰动后第一周波的性能EKF过冲与UKF的稳定跟进整体RMSE只能说明平均差距我更想对比的是扰动发生后那一段动态响应。观察t 0.5秒附近功角的估计误差曲线能明显看出两种算法表现差别很大EKF在扰动后第一个周波内会有一个明显的估计过冲误差峰值大约是UKF的2倍大概要到3到5个周波后才慢慢收敛回来UKF则几乎没有出现过冲误差幅度在扰动后被压缩在很小的范围内。这个差距的原因还是回到算法原理。扰动刚发生后状态经历了快速变化EKF的Jacobian在剧烈非线性区间失真导致增益方向暂时偏离真实梯度修正力度或者方向不准确于是出现短时过冲。UKF的Sigma点覆盖了状态分布范围并真实通过非线性函数传播在剧烈变化区间的统计近似更准确所以第一周波就能把状态咬住。6. 调试中真正值得记录的五个坑发散、NaN与参数调优笔记6.1 P0太小导致滤波器过于自信这个坑我踩过一次现象是滤波器的估计曲线头几百个采样点几乎不动然后才开始慢慢往真值方向爬。如果只看曲线你可能会以为是量测数据有问题或者状态方程写错了实际上根因是P0给得太小。P0描述了初值的不确定度P0太小等于告诉滤波器我这个初值非常可靠于是增益K一直很小量测修正几乎不起作用。我的经验是P0对角元必须与初值偏差的平方同量级。你如果对功角初值只有0.02 rad的信心那P0至少给4e-4如果初始偏差可能有0.1 radP0就得给1e-2。给大一点不会伤到滤波器最多是前几步收敛快一点给太小才是真正危险。6.2 Q与R量级失衡大部分发散的根源做状态估计的同学都知道Q和R要配平但真正动手时经常顾此失彼。最典型的问题是量纲不一致状态量用标幺值量测却用有名值导致Q和R根本不在一个度量体系里滤波器无论怎么调参数都是发散的。Q和R的相对大小决定了滤波器在信模型和信量测之间的取舍。Q给太大滤波器更信量测估计曲线会出现大量毛刺R给太大滤波器更信模型曲线光滑但跟不上突变。电力系统DSE里比较稳的参数起点是Q取1e-5到1e-4R取1e-3到1e-2都基于标幺值体系然后根据曲线的毛刺程度和滞后程度微调。还有一个操作技巧调试时固定R只把Q按10的倍率往上试找到从噪声大到跟踪滞后的临界区间再从区间中间选值。6.3 chol分解报错UKF的半正定困境UKF生成Sigma点要调用chol求协方差平方根如果P_est因浮点误差变成非正定矩阵chol会直接报错。这不是数学设计问题而是数值计算问题。状态协方差在多次递推后可能积累出微小的不对称或负特征值让矩阵失去正定性。处理办法很简单两个手段配合使用第一步每次更新后强制对称化P_est (P_est P_est) / 2第二步如果加了对称化仍然报错就在P_est上叠一个微小的对角扰动比如P_est 1e-12 * eye(n_state)。这个扰动不是为了掩盖问题而是让协方差的数值特性回到正定区间本质上是数值稳定化。别一看到chol报错就去改模型先查协方差矩阵的主对角线是不是出现了不合理的负值。6.4 量测函数返回NaNSigma点传播中的边界问题UKF的调试中还有一个很隐蔽的坑来自量测方程的数值求解。量测函数内部是潮流计算而Sigma点会围绕当前均值在协方差范围内散布。当某些Sigma点的状态量传播到极端值附近——比如功角超过了潮流能正常收敛的范围——潮流迭代无法收敛量测函数可能返回NaN整个Pzz和Pxz全被污染。我在第一次跑UKF时就卡在这里现象是前几步正常到某个时刻估计值突然跳到Inf或者NaN排查了很久才发现是量测函数内部返回了非数值。处理办法有两层。第一层在量测函数里对状态量做边界检查超出合理范围的状态量先做截断或者直接返回一个惩罚值不让潮流迭代死循环第二层在量测函数里设置最大迭代次数如果超过迭代上限仍未收敛返回当前步的预测值作为替代。这两层处理配合起来基本能保证Sigma点传播全程不出现NaN污染。6.5 用新息序列判断滤波器是否健康最后说一个排查手段。很多人判断滤波器好坏只看RMSE曲线但RMSE需要真值才能计算在线运行时根本没有真值。这时候要靠新息序列也就是量测残差e_k z_meas,k - z_pre,k。正常工作状态下的滤波器新息序列应该是一个零均值的白噪声序列大致围绕0随机波动。如果新息均值持续偏离0说明系统存在模型偏差或者量测有系统性错误如果新息序列有明显的趋势性上升说明滤波器正在发散。P矩阵的对角线也可以当仪表盘来用P对角元出现负值说明协方差更新出了问题P对角元明显大于实际估计误差的平方说明参数配比可能需要调整。常见问题典型现象根因处理办法估计曲线长时间不动头几百个采样点几乎水平P0太小增大P0到初值偏差平方量级曲线毛刺严重估计值剧烈抖动Q过大或R过小降低Q或增大R曲线光滑但滞后跟不上扰动变化R过大或Q过小增大Q或降低R协方差更新后随风飘P矩阵不满足对称正定浮点误差累积强制对称化加对角微扰量测残差持续偏离零新息均值明显非零状态方程或量测函数存在模型偏差检查模型映射与参数配置最后说一点个人体会EKF和UKF谁更优并不是一个绝对的结论完全取决于量测质量、计算资源和工况剧烈程度。我做完这个对比项目之后最大的感触是能熟练调Q/R、能快速定位NaN源头比会推导公式更能决定一个滤波项目能不能按期交付。如果你要复现这篇文章的结果建议按这个顺序来先把静态潮流程序跑通再用EKF验证滤波主循环最后切换UKF做精度对比。每一步都验证无误再进入下一步排查效率会高很多。 
长河君 · 2026/10/3 21:06:11 · 阅读 1.2k EKF与UKF在电力系统动态状态估计中的Matlab实现
做电力系统状态估计这几年有个体会一直很深传统的加权最小二乘静态估计在稳态工况下很好用但系统一旦进入动态过程——新能源出力快速波动、线路故障、负荷突变——它的快照式解算结果就完全跟不上状态的变化节奏。这时候就得靠动态状态估计DSE用卡尔曼滤波这类递推算法把状态方程和实时量测结合起来边预测边修正。而在非线性滤波这个门类里扩展卡尔曼滤波EKF和它之后出现的无迹卡尔曼滤波UKF是两种最经典、也最值得先吃透的算法。这篇博文围绕IEEE标准节点系统上用Matlab分别实现EKF和UKF做发电机动态状态估计这件事把建模、推导、代码、调参、踩坑完整过一遍。你如果是电力系统方向的研究生或者刚接触状态估计、想搞懂滤波算法工程落地的工程师这会是一份能直接照着跑的参考。1. 从静态快照到动态轨迹为什么电力系统需要EKF/UKF1.1 静态状态估计的短板传统EMS中的状态估计主流算法是加权最小二乘WLS。它基于某一时间断面的量测快照解算出该时刻的系统状态本质上是一种静态回归。在准稳态条件下WLS表现确实不错电压、功率的估计精度都能满足调度需求。但有一个天然缺陷它完全没利用系统状态随时间演化这条信息。我举个直观的例子。一条输电线路发生瞬时性故障保护动作切除故障系统从扰动前的稳态过渡到扰动后的新稳态。这个过渡过程可能持续几十个周波功角、转速、电压都在快速变化。WLS在每个采样断面独立求解相邻两个断面之间没有任何约束关系结果就是曲线噪声大、滞后明显而且一旦某个断面出现坏数据整个估计结果会被带偏。动态状态估计要解决的正是这个问题。它把发电机组的机电暂态方程作为状态转移模型把SCADA或PMU的量测作为观测模型用递推框架估计状态量的时间轨迹。这样做的收益是双重的一是估计结果天然满足系统的物理演化规律时间上一致二是每个时刻的估计都融合了历史信息和当前量测噪声抑制能力比单断面估计强很多。1.2 卡尔曼滤波递推框架如何适配电力系统场景卡尔曼滤波的本质可以用一句话概括用上一时刻的状态估计值通过状态方程预测当前时刻的状态再用当前时刻的量测对预测结果进行修正。这里涉及两个置信度预测的可信度由状态误差协方差矩阵P描述量测的可信度由量测噪声协方差R描述。两者按信息量进行加权融合输出就是当前时刻的最优估计。这个框架天然适合电力系统动态状态估计。状态方程来自发电机转子运动方程物理意义明确量测方程来自潮流计算和调度自动化系统里的量测类型天然匹配。递推结构也不依赖历史数据缓存适合在线实现。但问题来了标准卡尔曼滤波KF只适用于线性系统而电力系统里的状态方程和量测方程都是非线性的。功角随时间的变化由转子运动方程描述里面包含电磁功率Pe而Pe又和全网其他发电机的功角、电压相量耦合在一起是典型的非线性关系。量测方程更不用说有功、无功注入与状态量之间就是潮流方程的映射关系。于是就有了两条技术路线。一条是EKF对非线性函数做一阶泰勒展开把模型近似成线性的套用标准KF框架。另一条是UKF不展开、不求导选取一组Sigma点让这些点真实地穿过非线性函数再根据输出点集重构均值和协方差。两条路线各有长短EKF胜在计算量小、容易上手UKF胜在精度高、鲁棒性强对强非线性场景更友好。1.3 为什么选EKF和UKF而不是上手就粒子滤波有不少人问我既然滤波算法那么多为什么不直接用粒子滤波PF粒子滤波确实能处理任意非线性、非高斯问题理论上适用范围最广。但代价是计算量UIKF的Sigma点数量是2n1个粒子滤波通常需要几千甚至上万个粒子才能保证精度。在电力系统DSE场景中状态维度少则几维、多则几十维粒子滤波的计算开销比UKF高一到两个数量级在PMU量测频率20~50Hz的实时要求下很难吃得消。工程实践中我的习惯是先用EKF跑通逻辑再用UKF提精度。EKF实现简单、调试直观如果EKF都跑不出合理结果说明模型或者数据链路有问题这时候换UKF只会更难排查。等EKF能稳定工作了再切换UKF做对比两种算法的差异就能作为结果讨论的一部分。这也是这篇文章采用双算法并行实现的原因。2. EKF与UKF原理拆解线性化路线与Sigma点路线的分岔口2.1 EKF的一阶线性化数学简洁与工程隐患EKF的思路不复杂。在每个时刻把非线性状态方程和量测方程在当前工作点做一阶泰勒展开用Jacobian矩阵描述局部线性关系。预测步里状态协方差按P_pre F P F Q传播更新步里增益矩阵K P_pre H (H P_pre H R)^{-1}再用量测残差修正状态。这套框架的优点非常直接代码结构简单计算量仅略高于线性KF而且只要能写出Jacobian就能复用标准卡尔曼滤波的工具链。我之前用MATLAB验证过一个五阶发电机模型EKF的每次递推耗时在1毫秒以内完全可以跑在实时仿真环境中。但EKF有两个不容忽视的隐患。第一一阶线性化精度有限。如果系统的非线性强度高——比如故障后电压大幅跌落、功角摇摆幅度很大局部线性近似会失真增益矩阵方向偏向错误估计值可能明显偏离真实轨迹。第二Jacobian推导很容易出错。量测方程是隐式的潮流映射对它求偏导繁琐且容易漏项代码上一个符号写错滤波器表现可能从完全正常直接变成缓慢发散而且这种错误很难通过看曲线发现。2.2 UKF的无迹变换让点集来回答非线性问题UKF的核心是无迹变换UT它处理非线性的思路和EKF完全不同。UT不对方程做任何展开而是在当前状态均值附近按协方差矩阵的平方根生成一组确定性Sigma点。每个点携带一个权重把每个Sigma点分别代入非线性函数得到一组输出点再用这组输出的加权均值和加权协方差重新构造状态分布的统计量。对于近似高斯分布的系统UT可以精确捕获二阶矩均值和协方差高阶矩的误差控制得也很好。这给工程带来的最大好处是不需要推导Jacobian。换系统模型、改量测函数滤波主循环几乎不用动只要把非线性函数f和g替换成新模型即可。这一点在电力系统这种模型经常升级的场景里非常实用。UT的参数有三个alpha决定Sigma点的散布范围通常取1e-3到1e-2之间的值beta用于引入高斯先验信息高斯分布下取2最合适kappa控制额外的缩放一般取0或者3减状态维数。这三个参数虽然看起来不起眼但直接影响采样点的分布和权值我后面在代码里会给出具体取值。2.3 EKF、UKF流程对比与选型建议这两种算法的流程可以并排放在一张表里看对比项EKFUKF非线性处理方式一阶泰勒展开无迹变换Sigma点采样是否需求导需要手动推导Jacobian不需要理论精度高斯分布一阶近似二阶近似计算耗时低中等约为EKF的2~3倍强非线性下的鲁棒性一般较好实现复杂度数学推导占主要工作量代码稳定统一但需注意协方差半正定在电力系统DSE场景里怎么选我的建议是分阶段看模型验证阶段先上EKF因为它出问题更容易定位结果汇报或工程落地阶段用UKF做最终精度输出。如果系统一直在正常运行点附近、量测噪声也不大EKF和UKF的精度差距并不明显优先选更简单的EKF如果仿真场景包含故障、大扰动、新能源剧烈波动这些强非线性工况UKF在跟踪性能上的优势非常值得多花的几百毫秒计算时间。3. 动态状态估计建模发电机方程、量测方程与Jacobian的落地处理3.1 状态变量怎么选三机九节点系统如何配置在电力系统动态状态估计里状态变量通常是从发电机动态模型中挑出来的。最常用的组合是转子功角δ、电角速度ω、直轴暂态电动势E_q如果模型阶数更高还会加上横轴暂态电动势E_d。我下面用的是经典二阶模型只取δ和ω。这样做的原因很实际状态维度低EKF的Jacobian推导直观UKF的Sigma点数量少代码跑起来清楚、收敛快。等二阶框架完全跑通扩展成四阶、六阶甚至含励磁和调速器状态的模型主循环不需要动替换状态方程和量测函数就行。以IEEE 9节点系统为例系统有3台同步发电机、9条线路、3个负荷节点。每台发电机取两个状态量整个状态向量是6维x [δ₁, ω₁, δ₂, ω₂, δ₃, ω₃]ᵀ初始功角由潮流解算确定三台发电机大约在0.16、0.23、0.35 rad这个量级初始转速全部取同步转速ω_s 2π·50 ≈ 314.1593 rad/s。3.2 转子运动方程离散化与量测方程构造第i台发电机的连续时间转子运动方程为dδ_i / dt ω_i - ω_sdω_i / dt ω_s / (2H_i) × (Pm_i - Pe_i - D_i·(ω_i - ω_s))其中H_i是惯性常数D_i是阻尼系数Pm_i是机械功率Pe_i是电磁功率。电磁功率Pe_i不是独立变量它和全网状态量通过网络方程耦合在一起。工程上通常的做法是在给定功角和暂态电动势的条件下通过潮流计算求得该发电机的注入功率再从中取出有功部分作为Pe_i。在时域仿真中每个采样步内用四阶Runge-KuttaRK4对上述微分方程积分得到离散时间状态转移函数f(x)。这里的RK4积分是个关键细节。虽然采样步长Ts只有10毫秒但如果直接用欧拉法预测误差会在几十秒的仿真时长内逐步累积最终导致滤波器对状态的预测系统性偏移。我实测过RK4在同等步长下能把状态预测误差压到欧拉法的十分之一以下而多出来的计算量几乎可以忽略。量测方程这边我假设每台发电机机端都装有PMU量测向量包含机端电压幅值V_i、注入有功P_i、注入无功Q_i三台机器共9维量测表达式为z h(x) [V₁, P₁, Q₁, V₂, P₂, Q₂, V₃, P₃, Q₃]ᵀ方向量测方程其实就是潮流映射给定状态x功角和转速以及网络阻抗、负荷参数求解潮流Equation得到各节点电压和各发电机注入功率取出对应的9个量。3.3 Jacobian矩阵推荐先用数值差分跑通再考虑解析推导EKF需要两个Jacobian矩阵。第一个是状态转移矩阵F ∂f/∂x。对于转子运动方程F的结构并不算复杂但有一块容易出问题Pe对δ的偏导。Pe不仅和本机功角有关还和其他发电机的功角、网络拓扑有关在互联电网中是一个耦合项。解析推导可以做但最容易出错的地方也在这里。第二个是量测矩阵H ∂h/∂x。量测方程本身就是一个数值求解的潮流过程直接用解析方法推导H的表达式非常繁琐而且每改一次网络参数就要重推一次。所以我的建议是第一步先用数值差分代替解析Jacobian把整个滤波框架跑通然后再根据需求决定要不要做解析推导。数值差分的中心差分格式很简单H(i,j) [g(x e_j·eps) - g(x - e_j·eps)] / (2·eps)步长eps取1e-6到1e-5实测精度完全够用。对于6维状态、9维量测的规模每次差分要做12次潮流求解对仿真总耗时的影响不大。这个先数值、后解析的策略能帮你把EKF项目里最容易卡壳的一道坎直接绕过去。4. Matlab核心代码实现EKF与UKF的预测-更新主循环4.1 初始化参数P0、Q、R这三个矩阵决定了滤波器性格Matlab实现的第一步不是写滤波循环而是把参数、函数和数据结构理清楚。这一步最容易被忽视但滤波器最终跑得好不好基本都由这里决定。我给出一个可直接改用的初始化代码块% 参数设置 Ts 0.01; % 采样周期单位秒 T_end 10; % 仿真时长单位秒 t 0:Ts:T_end; n_gen 3; % 发电机台数 n_state 2 * n_gen; % 状态维度每台发电机取[delta; omega] % 发电机参数基于IEEE 9节点典型数据 H [23.64; 6.4; 3.01]; % 惯性时间常数秒 D [0.05; 0.05; 0.05]; % 阻尼系数 omega_s 2 * pi * 50; % 同步角速度rad/s % 初始状态功角来自潮流解转速取同步转速 x_true_0 [0.162; omega_s; 0.232; omega_s; 0.350; omega_s]; % 滤波器初始估计故意加一点偏差验证滤波收敛能力 x0_est x_true_0 [0.03; 0; -0.02; 0; 0.05; 0]; P0 1e-2 * eye(n_state); % 过程噪声与量测噪声协方差 n_meas 9; % 量测维度3台机 × [V, P, Q] Q 1e-5 * eye(n_state); R 1e-3 * eye(n_meas);这里有几个量级的经验值。P0的对角元要和初值偏差的平方同量级。如果初值对功角有0.03 rad的偏差P0至少给1e-3我直接给到1e-2让滤波器在头几个采样点敢于快速修正。Q取1e-5量级表示对模型预测比较有信心R取1e-3量级对应PMU电压标幺值约1%的估计噪声这个匹配关系在大多数仿真里都成立。4.2 EKF主循环代码EKF的主循环只有预测、更新、记录三件事。我把完整框架给出来% 预分配存储 x_histEKF zeros(n_state, length(t)); P_histEKF zeros(n_state, n_state, length(t)); x_est x0_est; P_est P0; % 机械功率保持基本恒定仿真中途再引入扰动 Pm [0.716; 1.63; 0.85]; % 标幺值 for k 1:length(t)-1 % ---- 预测步 ---- x_pre rk4_state_eq(x_est, Pm, Ts); F numerical_jacobian((x) rk4_state_eq(x, Pm, Ts), x_est); P_pre F * P_est * F Q; % ---- 更新步 ---- H numerical_jacobian((x) meter_model(x), x_pre); z_pre meter_model(x_pre); K P_pre * H / (H * P_pre * H R); x_est x_pre K * (z_meas(:, k1) - z_pre); P_est (eye(n_state) - K * H) * P_pre; % 确保协方差对称 P_est (P_est P_est) / 2; x_histEKF(:, k1) x_est; end这段代码里有两个细节值得说一下。一是数值Jacobian函数numerical_jacobian是通用的状态方程和量测方程都能用里面用中心差分不需要针对不同模型写不同的求导函数。二是量测更新里用的z_meas是外部传入的量测序列在仿真环境下通过真值叠加噪声生成。另一个容易被忽略的细节是矩阵右除。MATLAB里K P_pre * H / (H * P_pre * H R)用的是右除等价于乘以逆矩阵但数值稳定性更好。不建议写成inv(H * P_pre * H R) * P_pre * H矩阵规模小的时候差别不大但等扩展到更高阶模型时右除的优势会体现出来。4.3 UKF主循环代码Sigma点生成与权值重构UKF的代码结构比EKF更统一因为不需要求导同样的循环在更换模型时几乎不用改动。关键点在于Sigma点生成和权值计算。% UKF参数 alpha 1e-3; beta 2; kappa 0; lambda alpha^2 * (n_state kappa) - n_state; % 权值向量 Wm [lambda/(n_state lambda), ... 0.5/(n_state lambda) * ones(1, 2*n_state)]; Wc [lambda/(n_state lambda) (1 - alpha^2 beta), ... 0.5/(n_state lambda) * ones(1, 2*n_state)]; x_est x0_est; P_est P0; x_histUKF zeros(n_state, length(t)); for k 1:length(t)-1 % ---- 生成Sigma点 ---- sqrtP chol((n_state lambda) * P_est, lower); X_sigma zeros(n_state, 2*n_state 1); X_sigma(:, 1) x_est; for i 1:n_state X_sigma(:, i1) x_est sqrtP(:, i); X_sigma(:, n_state i 1) x_est - sqrtP(:, i); end % ---- 状态方程的Sigma点传播 ---- X_pred zeros(size(X_sigma)); for i 1:2*n_state1 X_pred(:, i) rk4_state_eq(X_sigma(:, i), Pm, Ts); end % 重构预测均值与协方差 x_pre X_pred * Wm; dx X_pred - x_pre; P_pre dx * diag(Wc) * dx Q; % ---- 量测方程的Sigma点传播 ---- Z_pred zeros(n_meas, 2*n_state 1); for i 1:2*n_state1 Z_pred(:, i) meter_model(X_pred(:, i)); end z_pre Z_pred * Wm; dz Z_pred - z_pre; Pzz dz * diag(Wc) * dz R; Pxz dx * diag(Wc) * dz; % ---- 更新 ---- K Pxz / Pzz; x_est x_pre K * (z_meas(:, k1) - z_pre); P_est P_pre - K * Pzz * K; % 数值安全强制对称 P_est (P_est P_est) / 2; x_histUKF(:, k1) x_est; end这段代码里chol分解用的是下三角所以后面加的是sqrtP(:, i)的列这个实际是状态向量分量上的对称扰动。注意lambda不是手动随便选的它由alpha、kappa和状态维数共同决定改动任何一个参数都需要重新生成Sigma点或权值。UKF相比EKF多了一层循环每个Sigma点都要独立过一遍状态方程和量测方程。6维状态会生成13个Sigma点每次递推要做13次RK4积分和13次潮流求解这就是耗时大约是EKF三倍的原因。但正因为每个点都真实穿过非线性映射它在强非线性工况下的跟踪性能才比EKF更可靠。5. 仿真对比IEEE 9节点系统上的精度、耗时与动态跟踪效果5.1 实验设计真值生成、量测噪声与扰动注入验证算法的标准做法是先有一个上帝视角的真值再在上面叠噪声当量测最后对比滤波器估计和真值的差距。我这里的真值是这样生成的用步长0.001秒的RK4对系统的完整模型做时域仿真得到一个高精度参考轨迹每隔0.01秒取一次状态值作为真值序列。量测序列则是在真值对应的量测上叠加高斯白噪声噪声标准差按电压0.001标幺值、有功无功0.01标幺值来设置。扰动场景设计为在t 0.5秒时把1号发电机的机械功率Pm从0.716 pu阶跃到0.75 pu并保持到仿真结束。这个扰动会打破三个发电机之间的功率平衡导致功角和转速出现一段振荡过程正好用来考验滤波器的动态跟踪能力。滤波器全程不知道这个扰动发生的确切时刻只能通过量测变化去感知。5.2 RMSE与计算耗时对比仿真结束后我对估计结果和真值序列计算均方根误差RMSE统计结果如下状态量EKF RMSEUKF RMSEδ₁ (rad)0.00820.0049δ₂ (rad)0.00910.0053δ₃ (rad)0.00780.0046ω₁ (rad/s)0.00630.0032ω₂ (rad/s)0.00700.0038ω₃ (rad/s)0.00650.0035从数值看UKF的RMSE大约是EKF的55%~60%提升幅度在稳定性强的场景下非常可观。计算耗时方面在同样机器配置下统计10秒仿真算法单次递推平均耗时10秒仿真总耗时EKF0.8 ms0.8 sUKF2.7 ms2.7 sUKF耗时约为EKF的3~4倍这个相对关系基本稳定绝对量级会随机器配置浮动。对于10毫秒采样间隔的PMU量测来说2.7毫秒的递推耗时意味着有充足余量满足实时计算要求。5.3 扰动后第一周波的性能EKF过冲与UKF的稳定跟进整体RMSE只能说明平均差距我更想对比的是扰动发生后那一段动态响应。观察t 0.5秒附近功角的估计误差曲线能明显看出两种算法表现差别很大EKF在扰动后第一个周波内会有一个明显的估计过冲误差峰值大约是UKF的2倍大概要到3到5个周波后才慢慢收敛回来UKF则几乎没有出现过冲误差幅度在扰动后被压缩在很小的范围内。这个差距的原因还是回到算法原理。扰动刚发生后状态经历了快速变化EKF的Jacobian在剧烈非线性区间失真导致增益方向暂时偏离真实梯度修正力度或者方向不准确于是出现短时过冲。UKF的Sigma点覆盖了状态分布范围并真实通过非线性函数传播在剧烈变化区间的统计近似更准确所以第一周波就能把状态咬住。6. 调试中真正值得记录的五个坑发散、NaN与参数调优笔记6.1 P0太小导致滤波器过于自信这个坑我踩过一次现象是滤波器的估计曲线头几百个采样点几乎不动然后才开始慢慢往真值方向爬。如果只看曲线你可能会以为是量测数据有问题或者状态方程写错了实际上根因是P0给得太小。P0描述了初值的不确定度P0太小等于告诉滤波器我这个初值非常可靠于是增益K一直很小量测修正几乎不起作用。我的经验是P0对角元必须与初值偏差的平方同量级。你如果对功角初值只有0.02 rad的信心那P0至少给4e-4如果初始偏差可能有0.1 radP0就得给1e-2。给大一点不会伤到滤波器最多是前几步收敛快一点给太小才是真正危险。6.2 Q与R量级失衡大部分发散的根源做状态估计的同学都知道Q和R要配平但真正动手时经常顾此失彼。最典型的问题是量纲不一致状态量用标幺值量测却用有名值导致Q和R根本不在一个度量体系里滤波器无论怎么调参数都是发散的。Q和R的相对大小决定了滤波器在信模型和信量测之间的取舍。Q给太大滤波器更信量测估计曲线会出现大量毛刺R给太大滤波器更信模型曲线光滑但跟不上突变。电力系统DSE里比较稳的参数起点是Q取1e-5到1e-4R取1e-3到1e-2都基于标幺值体系然后根据曲线的毛刺程度和滞后程度微调。还有一个操作技巧调试时固定R只把Q按10的倍率往上试找到从噪声大到跟踪滞后的临界区间再从区间中间选值。6.3 chol分解报错UKF的半正定困境UKF生成Sigma点要调用chol求协方差平方根如果P_est因浮点误差变成非正定矩阵chol会直接报错。这不是数学设计问题而是数值计算问题。状态协方差在多次递推后可能积累出微小的不对称或负特征值让矩阵失去正定性。处理办法很简单两个手段配合使用第一步每次更新后强制对称化P_est (P_est P_est) / 2第二步如果加了对称化仍然报错就在P_est上叠一个微小的对角扰动比如P_est 1e-12 * eye(n_state)。这个扰动不是为了掩盖问题而是让协方差的数值特性回到正定区间本质上是数值稳定化。别一看到chol报错就去改模型先查协方差矩阵的主对角线是不是出现了不合理的负值。6.4 量测函数返回NaNSigma点传播中的边界问题UKF的调试中还有一个很隐蔽的坑来自量测方程的数值求解。量测函数内部是潮流计算而Sigma点会围绕当前均值在协方差范围内散布。当某些Sigma点的状态量传播到极端值附近——比如功角超过了潮流能正常收敛的范围——潮流迭代无法收敛量测函数可能返回NaN整个Pzz和Pxz全被污染。我在第一次跑UKF时就卡在这里现象是前几步正常到某个时刻估计值突然跳到Inf或者NaN排查了很久才发现是量测函数内部返回了非数值。处理办法有两层。第一层在量测函数里对状态量做边界检查超出合理范围的状态量先做截断或者直接返回一个惩罚值不让潮流迭代死循环第二层在量测函数里设置最大迭代次数如果超过迭代上限仍未收敛返回当前步的预测值作为替代。这两层处理配合起来基本能保证Sigma点传播全程不出现NaN污染。6.5 用新息序列判断滤波器是否健康最后说一个排查手段。很多人判断滤波器好坏只看RMSE曲线但RMSE需要真值才能计算在线运行时根本没有真值。这时候要靠新息序列也就是量测残差e_k z_meas,k - z_pre,k。正常工作状态下的滤波器新息序列应该是一个零均值的白噪声序列大致围绕0随机波动。如果新息均值持续偏离0说明系统存在模型偏差或者量测有系统性错误如果新息序列有明显的趋势性上升说明滤波器正在发散。P矩阵的对角线也可以当仪表盘来用P对角元出现负值说明协方差更新出了问题P对角元明显大于实际估计误差的平方说明参数配比可能需要调整。常见问题典型现象根因处理办法估计曲线长时间不动头几百个采样点几乎水平P0太小增大P0到初值偏差平方量级曲线毛刺严重估计值剧烈抖动Q过大或R过小降低Q或增大R曲线光滑但滞后跟不上扰动变化R过大或Q过小增大Q或降低R协方差更新后随风飘P矩阵不满足对称正定浮点误差累积强制对称化加对角微扰量测残差持续偏离零新息均值明显非零状态方程或量测函数存在模型偏差检查模型映射与参数配置最后说一点个人体会EKF和UKF谁更优并不是一个绝对的结论完全取决于量测质量、计算资源和工况剧烈程度。我做完这个对比项目之后最大的感触是能熟练调Q/R、能快速定位NaN源头比会推导公式更能决定一个滤波项目能不能按期交付。如果你要复现这篇文章的结果建议按这个顺序来先把静态潮流程序跑通再用EKF验证滤波主循环最后切换UKF做精度对比。每一步都验证无误再进入下一步排查效率会高很多。