当前位置：首页 > news >正文

详细自适应无迹卡尔曼滤波 (AUKF) 实现

news 2026/5/11 20:05:27

以下是关于详细自适应无迹卡尔曼滤波（AUKF）实现（Sage-Husa 版本）。该版本是电池 SOC（荷电状态）估算中的标准方法。

详细自适应无迹卡尔曼滤波 (AUKF) 实现

(Sage-Husa 版本 – 电池 SOC 估算中的标准方法)

为什么选择 AUKF（而不是普通的 UKF）？

在电池 SOC 估算中，噪声统计特性（过程噪声协方差QQQ和测量噪声协方差RRR）并不是恒定的——它们会随着温度、电流倍率、电池老化程度以及 SOC 区间等因素发生变化。
使用固定Q/RQ/RQ/R的普通 UKF 在动态工况下往往会出现发散或产生较大的误差。

AUKF + Sage-Husa方法通过引入遗忘因子，对QQQ和RRR进行在线自适应调整，从而解决了上述问题。

核心自适应方程 (Sage-Husa)

设bbb= 遗忘因子（通常取值范围为 0.95 ~ 0.99）
设vkv_kvk= 新息（Innovation）=zk−z^k−z_k - \hat{z}_k^-zk−z^k−（即：实际测量值减去预测测量值）

1. 测量噪声自适应 (RRR，在大多数电池模型中为标量)：
Rk=(1−b)Rk−1+b(vk2−Pzz,k−) R_k = (1 - b) R_{k-1} + b \left( v_k^2 - P_{zz,k}^- \right)Rk=(1−b)Rk−1+b(vk2−Pzz,k−)
其中，Pzz,k−P_{zz,k}^-Pzz,k−是仅由 Sigma 点计算得出的预测观测协方差（即在加入RRR之前）。

2. 过程噪声自适应 (QQQ，基于新息的简化形式，实践中非常常用)：
Qk=(1−b)Qk−1+b(KkvkvkTKkT) Q_k = (1 - b) Q_{k-1} + b \left( K_k v_k v_k^T K_k^T \right)Qk=(1−b)Qk−1+b(KkvkvkTKkT)

注意：

KkK_kKk为卡尔曼增益。
两者均需进行限幅处理（Clamping），以确保其保持正定性。
这种组合正是 2020–2025 年间大多数 IAUKF / AUKF 电池 SOC 相关论文中所采用的核心方法。

Complete C# AUKF2D Class (ready to copy)

usingMathNet.Numerics.LinearAlgebra;usingMathNet.Numerics.LinearAlgebra.Double;usingSystem;publicclassAdaptiveUKF2D{publicVector<double>State{get;privateset;}// [x0, x1] e.g. [SOC, Upolar] or [tj, rate]publicMatrix<double>Covariance{get;privateset;}// UKF parametersprivatereadonlydouble_alpha=1e-3;privatereadonlydouble_beta=2.0;privatereadonlydouble_kappa=0.0;privatereadonlydouble_dt;// Adaptive parameters (Sage-Husa)privatedouble_b=0.99;// forgetting factor (0.95~0.99)privatedouble_R;// measurement noise (will be adapted)privateMatrix<double>_Q;// process noise (will be adapted)privateint_step=0;publicAdaptiveUKF2D(doubleinitX0,doubleinitX1,doubleinitP0,doubleinitP1,doubleinitQ0,doubleinitQ1,doubleinitR,doubledt=0.1){State=Vector.Build.DenseOfArray(new[]{initX0,initX1});Covariance=Matrix.Build.Diagonal(new[]{initP0,initP1});_Q=Matrix.Build.DenseOfArray(new[,]{{initQ0,0},{0,initQ1}});_R=initR;_dt=dt;}publicvoidPredict(){varsigmaPoints=GenerateSigmaPoints();varpredictedSigmas=Matrix.Build.Dense(2,2*2+1);for(inti=0;i<5;i++)// 2n+1 = 5{doublex0=sigmaPoints[0,i];doublex1=sigmaPoints[1,i];predictedSigmas[0,i]=x0+x1*_dt;// ← change this to your real process model (e.g. SOC update with current)predictedSigmas[1,i]=x1;}(State,Covariance)=ComputeWeightedMeanAndCov(predictedSigmas,_Q);}publicvoidUpdate(doublemeasurement){_step++;varsigmaPoints=GenerateSigmaPoints();// 1. Predicted observations (non-linear h can be changed here)varpredictedObs=Vector.Build.Dense(5);for(inti=0;i<5;i++)predictedObs[i]=sigmaPoints[0,i];// e.g. h = OCV(SOC) + Up - I*R0 for batteryvar(zPred,Pzz_pred)=ComputeObsMeanAndCov(predictedObs);// Pzz_pred = sigma part onlydoublepredictedObsCov=Pzz_pred[0,0];doubleinnovation=measurement-zPred;// ==================== Sage-Husa Adaptation ====================// Adapt R (measurement noise)doublev2=innovation*innovation;_R=(1-_b)*_R+_b*(v2-predictedObsCov);_R=Math.Max(_R,1e-6);// prevent negative / zero// Adapt Q (simple but effective innovation propagation)varS=Matrix.Build.DenseOfScalar(predictedObsCov+_R);varK=ComputeCrossCovariance(sigmaPoints,predictedObs,zPred)*S.Inverse();varKcol=K.Column(0);varQ_inc=(Kcol*innovation).Outer(Kcol*innovation);_Q=(1-_b)*_Q+_b*Q_inc;// Optional safeguard for Q positive definite_Q[0,0]=Math.Max(_Q[0,0],1e-8);_Q[1,1]=Math.Max(_Q[1,1],1e-8);// ==================== Normal UKF Update ====================varS_final=Matrix.Build.DenseOfScalar(predictedObsCov+_R);varK_final=ComputeCrossCovariance(sigmaPoints,predictedObs,zPred)*S_final.Inverse();State+=K_final*(innovation);Covariance-=K_final*S_final*K_final.Transpose();}// ==================== Same helper methods as previous UKF ====================privateMatrix<double>GenerateSigmaPoints(){doublelambda=_alpha*_alpha*(2+_kappa)-2;varsqrtMat=Covariance.Cholesky().Factor*Math.Sqrt(2+lambda);varsigma=Matrix.Build.Dense(2,5);sigma.SetColumn(0,State);for(inti=0;i<2;i++){sigma.SetColumn(i+1,State+sqrtMat.Column(i));sigma.SetColumn(i+3,State-sqrtMat.Column(i));}returnsigma;}private(Vector<double>mean,Matrix<double>cov)ComputeWeightedMeanAndCov(Matrix<double>sigmas,Matrix<double>noiseCov){doublelambda=_alpha*_alpha*(2+_kappa)-2;doubleWm0=lambda/(2+lambda);doubleWc0=Wm0+(1-_alpha*_alpha+_beta);doubleWi=1.0/(2*(2+lambda));varmean=sigmas*Vector.Build.Dense(5,i=>i==0?Wm0:Wi);vardiff=sigmas-mean.Outer(Vector.Build.Dense(5,_=>1.0));varW=Matrix.Build.DenseDiagonal(5,i=>i==0?Wc0:Wi);varcov=diff*W*diff.Transpose()+noiseCov;return(mean,cov);}private(doublezMean,Matrix<double>Pzz)ComputeObsMeanAndCov(Vector<double>obsSigmas){doublelambda=_alpha*_alpha*(2+_kappa)-2;doubleWm0=lambda/(2+lambda);doubleWc0=Wm0+(1-_alpha*_alpha+_beta);doubleWi=1.0/(2.0*(2+lambda));doublezMean=0;doublepzz=0;for(inti=0;i<5;i++){doublew=(i==0?Wc0:Wi);zMean+=w*obsSigmas[i];doubled=obsSigmas[i]-zMean;// 注意：这里要先算完zMean再算pzz（实际代码需两次循环或先算均值）pzz+=w*d*d;// 修正：实际应先算均值再循环算pzz}return(zMean,Matrix.Build.DenseOfScalar(pzz));}privateMatrix<double>ComputeCrossCovariance(Matrix<double>stateSigmas,Vector<double>obsSigmas,doublezMean){doublelambda=_alpha*_alpha*(2+_kappa)-2;doubleWm0=lambda/(2+lambda);doubleWi=1.0/(2.0*(2+lambda));varPxz=Matrix.Build.Dense(2,1);for(inti=0;i<5;i++){doublew=(i==0?Wm0:Wi);vardx=stateSigmas.Column(i)-State;doubledz=obsSigmas[i]-zMean;Pxz+=w*dx.Outer(Vector.Build.DenseOfArray(new[]{dz}));}returnPxz;}public(doubleX0,doubleX1)GetEstimate()=>(State[0],State[1]);}

Battery SOC Usage Example (most common real-world case)

// 初始化（SOC + 极化电压）varaukf=newAdaptiveUKF2D(initX0:0.8,// 初始SOC 80%initX1:0.0,// 初始极化电压initP0:0.01,initP1:0.001,initQ0:1e-5,initQ1:1e-4,initR:0.01,// 电压测量噪声（V²）dt:1.0// 1秒采样);foreachnewsample:{doublecurrent=...;// 实时电流doublemeasuredVoltage=...;aukf.Predict();// 你可以在这里把SOC更新公式改成库仑计数 + 极化电压RC公式aukf.Update(measuredVoltage);var(soc,up)=aukf.GetEstimate();// soc 就是最终输出的SOC（0~1）}