鲁棒迭代扩展卡尔曼滤波（RIEKF）设计与Matlab实现

大概在半年前，我帮一位做电力系统仿真的同学排查过一个奇怪的问题：动态状态估计的滤波曲线在某一拍突然跳出一个尖峰，然后花了好几百毫秒才慢慢回到真值附近。看代码，标准扩展卡尔曼滤波（EKF）的流程没毛病，量测数据画出来才发现，PMU遥测通道里混入了几个明显偏离正常范围的坏数据。这个经历让我对电力系统动态状态估计有了一个很深的印象：如果只按高斯白噪声假设设计滤波器，那么“鲁棒”就不是一个可选项，而是实际运行中的必需品。这篇文章围绕鲁棒迭代扩展卡尔曼滤波（RIEKF）的设计思路、公式推导和Matlab代码实现展开，聊聊它如何同时应对非线性系统模型和含坏数据的量测场景。如果你正在复现状态估计算法、准备做算法对比，或者想为后续研究打基础，这篇应该能帮你省不少时间。

1. 动态量测噪声的本质：为什么教科书假设在电网现场常常失效

1.1 几类典型的量测异常

电力系统的量测来源主要是SCADA和PMU。传统静态状态估计多基于SCADA的秒级断面数据，量测更新频率低，而且经过了一定的前置滤波处理，噪声相对平滑；而动态状态估计面向的是PMU提供的快速同步相量数据，采样率通常为25或50帧每秒，时间尺度接近机电暂态过程，对量测质量的敏感度要高得多。

在实际数据流里，除了常规的热噪声之外，还有几类很难用高斯分布描述的异常。通信毛刺是其中最典型的一种，同步相量数据经过网络传输时会丢包、误码，表现为个别采样点上幅值或相角突变；大型设备启停、电磁干扰会耦合进测量回路，造成短时间尖峰；故障或操作暂态中，量测值剧烈变化，可能被保护装置截断、延时，甚至混入暂态分量。还有一类是通道老化与校准偏差，长期运行后部分相量测量单元会缓慢漂移，形成持续的系统性偏置。

这些异常如果简单当作“方差更大的高斯噪声”处理，通常会遇到两个问题：一是方差本身难以在线估计，个别离群点对方差估计的影响非常巨大；二是滤波器的更新方程对离群点没有任何防御机制。这也是为什么鲁棒状态估计在电力系统里不是一个空泛的概念，而是直接关系到估计结果能否被调度和监控系统采信的核心问题。

1.2 平方损失为什么会被单个离群点撬动

再看EKF的量测更新，本质上是在做最小二乘式的状态修正。如果某一通道的残差服从零均值高斯分布，常规点的平方损失是残差平方除以量测方差的量级。当注入一个坏数据使得残差达到正常标准差的20倍时，平方损失会放大到400倍。换句话说，这一个异常样本点在损失函数中的权重，相当于几百个正常样本的总和。

我做过一个很简单的实验：真值为1.0、噪声标准差0.1，某一拍把量测直接拉到-8.0。普通EKF这一拍估计值瞬间偏移好几个标准差，而且因为协方差也被异常更新扭曲，后面几拍还得重新收敛。动态估计恰恰最在意暂态过程中快速变化的跟踪能力，如果关键窗口被一个坏数据污染，后续控制决策就很容易建立在错误的估计值上。这个例子解释了为什么滤波算法需要从目标函数层面对异常值进行抑制，而不是单纯寄希望于数据清洗。

2. RIEKF的设计思路：从EKF到迭代再到鲁棒的三层递进

2.1 标准EKF的局限：一阶线性化的精度边界

标准EKF在预测阶段直接用非线性状态转移函数传播状态均值，但在更新阶段只在一个“预测点”处做一阶泰勒展开，计算量测雅可比矩阵，然后执行一次量测更新。这种做法隐含一个前提：系统的非线性程度在这个局部区间内足够弱，一阶近似可以接受。

在电力系统动态状态估计里，这个前提经常不成立。发电机转子运动方程包含转速与功角的乘积项，量测方程中的功率又与功角的正弦、余弦直接相关，大扰动暂态过程中功角摆动幅度可能达到几十度，此时sin和cos的线性化截断误差已经不可忽视。若量测更新又恰好在系统轨迹曲率最大的时刻进行，单次线性化会带来明显偏差，进而污染整个递推过程。

2.2 迭代EKF（IEKF）的改进：让线性化点贴近真实状态

迭代扩展卡尔曼滤波的思路并不复杂：既然一次线性化不准，那就在一次量测更新中反复做“线性化—滤波更新”的循环，让参与线性化的状态点逐步向当前量测所对应的最大后验估计点靠近。可以打一个比方：标准EKF是闭着眼一步跳到目标附近，IEKF则是睁着眼走几步，每走一步重新看一次方向，自然更稳妥。

IEKF的迭代更新公式通常写成这样的一种形式：

x_{i+1} = x_pred + K_i [ z - h(x_i) - H_i (x_pred - x_i) ]

其中x_i是第i次迭代时的估计值，H_i是围绕x_i重新计算的量测雅可比矩阵。注意括号里的修正项H_i(x_pred - x_i)，这是IEKF和普通“重复滤波”的关键区别：它补偿了当前迭代点与预测点之间的差异，保证整个更新步沿着Gauss-Newton方向逼近非线性最小二乘解。少了这一项，迭代过程不一定收敛到正确解，后面我会在代码部分再强调。

2.3 从最小二乘到Huber：鲁棒化的关键一步

迭代能改善线性化误差，但治不了坏数据。坏数据的影响来自目标函数本身——平方损失对极端残差过于敏感。鲁棒化的核心就是换掉这个损失函数，最常用也最容易实现的是Huber函数：

• 当|r|小于等于阈值γ时，使用平方项0.5r^2，保证高斯噪声背景下接近最小二乘的统计效率；
• 当|r|大于γ时，改用线性惩罚γ|r|减去常数0.5γ^2，让极端残差只产生线性影响。

和Huber函数对应的等价权重为：|r|不超过γ时权重为1，超过γ时权重为γ/|r|。权重大于0且小于1，残差越极端，权重越小。把这个权重乘到量测更新中，就相当于自动把坏数据的增益压下去。γ的取值在经典稳健统计中通常取1.345，此时高斯噪声下算法仍能保持约95%的效率；若实际场景中坏数据比例很高，可以对γ再做下调，但代价是正常噪声下的估计效率会下降。

2.4 适合电力系统动态估计的状态空间模型

为了把RIEKF落到Matlab里，需要一个具体模型。我推荐从单机无穷大系统的简化三阶发电机模型入手，状态向量取x = [δ; ω; E_q′]，其中δ是功角，ω是转速，E_q′是暂态电势。连续时间状态方程为：

dδ/dt = ω - ω_s

dω/dt = (1/(2H)) [P_m - P_e - D(ω - ω_s)]

dE_q′/dt = (1/T_d0′) [E_fd - E_q′ - (X_d - X_d′) I_d]

这里H是惯性时间常数，D是阻尼系数，T_d0′是励磁绕组时间常数，P_m是机械功率，P_e是电磁功率，E_fd是励磁电压。量测方程则取PMU能够直接观测的量，比如功角δ、角频率偏差以及有功功率P_e。有了连续模型之后，再用欧拉法或四阶Runge-Kutta法做离散化。

雅可比矩阵F和H既可以解析推导，也可以用中心差分做数值计算。我的建议是：仿真调试阶段先用解析推导，这样后续改进算法时不会把模型错误和滤波问题混在一起；如果模型太复杂，再用数值差分交叉验证。

2.5 RIEKF的完整流程

整个RIEKF的单步递推可以归纳成下面几个步骤。先做状态预测和协方差预测；然后进入迭代循环，在循环里计算当前估计点的量测雅可比矩阵，计算量测残差，用残差协方差对残差做归一化，再按Huber规则计算每个量测通道的权重；接着用权重修正量测噪声协方差，计算滤波增益，更新状态；循环结束后用最后一次的增益和雅可比更新协方差。这个流程把迭代线性化和鲁棒加权组合在一起，既解决非线性问题，又压制离群值。

3. MATLAB实现细节：核心代码与仿真脚本拆解

3.1 仿真模型搭建的起点：单机无穷大系统

有人会觉得直接上多机系统才贴近实际，但我的调试经验是：状态维数越高，问题定位越难。单机无穷大系统只有3个状态量，物理概念直观，跟踪曲线也容易和真值对照，非常适合先验证RIEKF的算法行为。

仿真参数可以这样设置：采样周期Δt取0.02秒，对应PMU的50帧每秒；惯性常数H取5.0秒，阻尼系数D取2.0，T_d0′取7.0秒，直轴同步电抗X_d取1.2，直轴暂态电抗X_d′取0.4。这些参数取自典型发电机数量级，不是某个实际设备，但足够反映暂态过程的动态特性。量测噪声标准差按通道分别设置，功角和角频率通道取较小值，功率通道取相对较大值。

3.2 量测生成与坏数据注入

仿真测试的第一步是生成量测序列。先由状态方程推出一条“真值轨迹”，再在真值上加高斯噪声，按一定概率注入坏数据。坏数据的注入方式很关键：不是简单把噪声方差调大，而是以某个概率将特定量测点替换成严重偏离的值，这样才能检验滤波器对离群点的真实反应。

matlab复制% 生成含坏数据的量测序列
z_true = zeros(m, N);
z_meas = zeros(m, N);
for k = 1:N
    z_true(:, k) = h(x_true(:, k));
    z_meas(:, k) = z_true(:, k) + sqrt(R) * randn(m, 1);
    for i = 1:m
        if rand < p_bad   % p_bad 为坏数据概率
            z_meas(i, k) = z_true(i, k) + bad_scale * abs(z_true(i, k)) * sign(randn);
        end
    end
end

p_bad一般设0.02，即2%的量测点被污染；bad_scale取3到10，表示异常幅度是正常量值的若干倍。调试时先固定随机种子，保证每次实验的坏数据位置一致，方便对比算法差异。

3.3 状态预测与协方差传播

预测阶段用离散化的状态转移函数。对于三阶模型，一步欧拉法已经能给出不错的结果，但如果采样间隔较大，建议用四阶Runge-Kutta；我测试下来，0.02秒采样间隔下两者差别不大，如果以后要做网络化扩展，直接保留功能函数接口更方便。

matlab复制% 状态预测
x_pred = f_state(x_est);
% 协方差预测
F = df_dx(x_est);
P_pred = F * P_est * F' + Q;

Q的设置有一点讲究：如果Q太小，滤波器会过度信任模型，量测稍微异常就产生持续偏差；如果Q太大，状态轨迹会出现明显抖动。可以先按状态量典型变化幅度的平方来初始化Q，再根据新息序列做微调。

3.4 RIEKF迭代更新与Huber加权

这是整个代码的核心。量测更新不再是一步完成，而是在迭代中反复计算雅可比矩阵和Huber权重。等效噪声协方差R_modified的计算是整个鲁棒策略的载体：当某个量测通道权重w_i小于1时，相当于该通道的量测噪声方差被放大，滤波器自动降低对这个通道的信任。

matlab复制% RIEKF迭代量测更新
x_iter = x_pred;
for iter = 1:N_iter
    H = dh_dx(x_iter);              % 在当前迭代点线性化
    z_hat = h(x_iter);
    res = z_meas - z_hat;           % 量测残差
    S = H * P_pred * H' + R;        % 残差协方差

    % 计算Huber权重
    w = ones(m, 1);
    for i = 1:m
        r_norm = abs(res(i)) / sqrt(S(i, i));
        if r_norm > gamma
            w(i) = gamma / r_norm;
        end
    end

    % 用权重修正量测噪声协方差
    R_modified = R / diag(w);

    % 滤波增益
    K = P_pred * H' / (H * P_pred * H' + R_modified);

    % 带Gauss-Newton修正项的迭代更新
    innov = res - H * (x_pred - x_iter);
    x_iter = x_pred + K * innov;
end
x_est = x_iter;
P_est = (eye(n) - K * H) * P_pred;

这段代码有几处特别容易写错。第一，更新公式里的修正项H * (x_pred - x_iter)不能省，有些资料里为了简化会把迭代写成x_iter = x_iter + K * res，那本质上只是在重复同一个滤波步，无法体现Gauss-Newton的收敛性质。第二，Huber权重归一化时用的是残差协方差S的对角元，而不是直接用残差绝对值；如果量测通道量纲差异很大，不归一化会导致阈值γ在不同通道上的含义不一致。第三，R_modified用的是R除以权重，不是R乘以权重，方向反了效果会完全相反。

3.5 评价指标的计算

做算法对比不能只看曲线，要有一个量化指标。最常用的是均方根误差RMSE，以及最大绝对误差。RMSE反映整体跟踪精度，最大绝对误差反映最坏情况下的偏离程度，两个指标互补。

matlab复制rmse = sqrt(mean((x_est - x_true).^2, 2));
max_abs_err = max(abs(x_est - x_true), [], 2);

输出指标时按状态量分别统计，不要只给一个综合值。功角、转速和暂态电势的物理单位不同，混在一起会掩盖问题。我在一次测试中遇到过功角RMSE很小但暂态电势RMSE偏大的情况，分开统计后很快就定位到是量测方程里功率项的权重设置不合理。

4. 数值仿真对比：坏数据场景下的精度与稳定性

4.1 测试场景和参数配置

为了对比标准EKF、IEKF和RIEKF，我设置了一个统一场景：单机无穷大系统，仿真时长1秒，采样间隔0.02秒，共50个采样点。量测通道为功角、角频率和电磁功率，量测噪声标准差分别取0.001弧度、0.001弧度每秒和0.01标幺值。坏数据概率为2%，异常幅度为正常量值的5倍。RIEKF的迭代次数取3，Huber阈值γ取1.345。

这样的设置下，坏数据虽然只占少数几个采样点，但已经足以让普通EKF明显偏离真值。因为测试场景很小，随机种子固定后结果完全可复现，调试时建议把坏数据的位置打出来，确认算法确实在异常点附近做出了降权处理。

4.2 三种算法在两种场景下的表现

先看不含坏数据的情况，三者都能跟踪真值，精度差距在10%以内。RIEKF因为Huber函数在高斯噪声下只保留约95%效率，RMSE比EKF略高一点，这是为鲁棒性付出的代价，属于正常现象。

再看2%坏数据场景，差距就很明显了。标准EKF在坏数据出现的拍子上会产生明显尖峰，最大功角偏差达到0.6标幺值左右，并且由于协方差受到污染，后续几拍依然残留偏差；IEKF因为迭代线性化的关系，峰值略低一些，但本质上仍然是二次损失，无法抵抗离群值；RIEKF则把最大偏差压到了0.1标幺值以内，RMSE基本保持在接近正常场景的水平。可以看出，迭代和鲁棒是两个正交的改进方向，只做迭代不解决坏数据问题，只做鲁棒不迭代又不能处理强非线性，两者结合才有效。

这里给出的是模拟算例的相对量级，不同系统参数下数值会有变化，但趋势是稳定的。我实际观察到的滤波曲线上，RIEKF并非完全无视坏数据，而是在坏数据拍产生一个很小的波动后立即回归真值，不会出现持续偏离。这个行为正是动态状态估计在工程上最需要的特性。

4.3 关键参数调试：Huber阈值、迭代次数和噪声协方差

Huber阈值γ是鲁棒性和效率之间的天平。γ取0.5时，正常高斯噪声下的估计效率明显下降，滤波曲线偏保守；γ取3时，几乎退化成标准EKF，坏数据抑制能力消失。我测试下来，在2%坏数据比例下，γ在1.0到1.5之间的综合表现最好，这也和经典稳健统计的结论一致。

迭代次数N_iter对算法效果的影响呈现饱和趋势。第一次迭代带来的改善最大，第二次次之，到第三次之后几乎没有明显变化，反而增加计算量。建议在线应用时取3次，离线仿真可以适当取5次做更精细的结果。Q和R的取值同样重要，Q设置过小会让滤波器对新息过于迟钝，坏数据出现后无法快速纠偏；Q设置过大则正常量测下抖动明显，需要在仿真中反复试凑。

5. 复现排坑与工程落地建议

5.1 滤波初值和发散保护

动态状态估计的滤波器必须从某个初始状态启动。初值离真值太远时，滤波器在头几拍会产生较大误差，如果此时又遇上坏数据，很可能直接发散。最简单的处理是直接用PMU首帧量测反推初始状态，或者先跑一小段标准EKF做“暖启动”，等协方差稳定后再切换RIEKF。

在仿真中加入发散保护也很有必要。比如监控新息序列的幅值，连续多拍超过3倍残差标准差就主动重置协方差矩阵。这个逻辑在标准EKF里是锦上添花，在鲁棒滤波里同样重要，因为当坏数据比例升高时，即使Huber权重能压制单点异常，滤波器仍可能因为模型失配而出现发散倾向。

5.2 雅可比、残差归一化与迭代修正项：三个最容易写错的地方

第一处是雅可比矩阵。手动推导很容易漏项，尤其是功率对功角的偏导里包含多个中间变量。我建议写一个函数用中心差分计算数值雅可比，和解析结果做一次对比，误差在1e-6量级才算通过。第二处是残差归一化。我在3.4节已经提过，必须用残差协方差S的对角元做归一化，而不是直接对原始残差取阈值。第三处是迭代修正项，很多网上的示例代码会把IEKF简写成重复执行普通更新，那是错的。判断标准很简单：如果迭代3次和迭代1次结果几乎没有差别，大概率是修正项没写对。

5.3 从单机算例走向工程化的扩展思路

单机算例跑通后，RIEKF可以往两个方向扩展。一个方向是多机系统，状态维数上升后，雅可比矩阵的计算和协方差传播都会变得复杂，但算法框架不需要改变，只需要把状态转移函数和量测函数替换成多机模型。另一个方向是事件触发的鲁棒更新，即正常工况下直接跑标准EKF，只有检测到新息异常时才启用Huber加权和迭代循环，这样可以明显降低计算开销。

我在实际测试中最大的体会是，鲁棒滤波本质上是“用一点点高斯噪声效率换整体稳定性”。RIEKF的实现难度并不高，核心难点在于理解迭代更新与鲁棒加权之间的逻辑关系。调试时建议保留量测时序曲线，配合新息序列观察异常点出现在哪里，这比只看RMSE数字更能定位问题。把代码拆成状态函数、雅可比函数、滤波器主体三个模块，改参数做对比会非常省事，后面如果要把算法做成实时版本，也能直接复用这些模块。