 实现)
以下是关于详细自适应无迹卡尔曼滤波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 新息Innovationzk−z^k−z_k - \hat{z}_k^-zk−z^k−即实际测量值减去预测测量值1. 测量噪声自适应 (RRR在大多数电池模型中为标量)Rk(1−b)Rk−1b(vk2−Pzz,k−) R_k (1 - b) R_{k-1} b \left( v_k^2 - P_{zz,k}^- \right)Rk(1−b)Rk−1b(vk2−Pzz,k−)其中Pzz,k−P_{zz,k}^-Pzz,k−是仅由 Sigma 点计算得出的预测观测协方差即在加入RRR之前。2. 过程噪声自适应 (QQQ基于新息的简化形式实践中非常常用)Qk(1−b)Qk−1b(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−1b(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{publicVectordoubleState{get;privateset;}// [x0, x1] e.g. [SOC, Upolar] or [tj, rate]publicMatrixdoubleCovariance{get;privateset;}// UKF parametersprivatereadonlydouble_alpha1e-3;privatereadonlydouble_beta2.0;privatereadonlydouble_kappa0.0;privatereadonlydouble_dt;// Adaptive parameters (Sage-Husa)privatedouble_b0.99;// forgetting factor (0.95~0.99)privatedouble_R;// measurement noise (will be adapted)privateMatrixdouble_Q;// process noise (will be adapted)privateint_step0;publicAdaptiveUKF2D(doubleinitX0,doubleinitX1,doubleinitP0,doubleinitP1,doubleinitQ0,doubleinitQ1,doubleinitR,doubledt0.1){StateVector.Build.DenseOfArray(new[]{initX0,initX1});CovarianceMatrix.Build.Diagonal(new[]{initP0,initP1});_QMatrix.Build.DenseOfArray(new[,]{{initQ0,0},{0,initQ1}});_RinitR;_dtdt;}publicvoidPredict(){varsigmaPointsGenerateSigmaPoints();varpredictedSigmasMatrix.Build.Dense(2,2*21);for(inti0;i5;i)// 2n1 5{doublex0sigmaPoints[0,i];doublex1sigmaPoints[1,i];predictedSigmas[0,i]x0x1*_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;varsigmaPointsGenerateSigmaPoints();// 1. Predicted observations (non-linear h can be changed here)varpredictedObsVector.Build.Dense(5);for(inti0;i5;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 onlydoublepredictedObsCovPzz_pred[0,0];doubleinnovationmeasurement-zPred;// Sage-Husa Adaptation // Adapt R (measurement noise)doublev2innovation*innovation;_R(1-_b)*_R_b*(v2-predictedObsCov);_RMath.Max(_R,1e-6);// prevent negative / zero// Adapt Q (simple but effective innovation propagation)varSMatrix.Build.DenseOfScalar(predictedObsCov_R);varKComputeCrossCovariance(sigmaPoints,predictedObs,zPred)*S.Inverse();varKcolK.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_finalMatrix.Build.DenseOfScalar(predictedObsCov_R);varK_finalComputeCrossCovariance(sigmaPoints,predictedObs,zPred)*S_final.Inverse();StateK_final*(innovation);Covariance-K_final*S_final*K_final.Transpose();}// Same helper methods as previous UKF privateMatrixdoubleGenerateSigmaPoints(){doublelambda_alpha*_alpha*(2_kappa)-2;varsqrtMatCovariance.Cholesky().Factor*Math.Sqrt(2lambda);varsigmaMatrix.Build.Dense(2,5);sigma.SetColumn(0,State);for(inti0;i2;i){sigma.SetColumn(i1,StatesqrtMat.Column(i));sigma.SetColumn(i3,State-sqrtMat.Column(i));}returnsigma;}private(Vectordoublemean,Matrixdoublecov)ComputeWeightedMeanAndCov(Matrixdoublesigmas,MatrixdoublenoiseCov){doublelambda_alpha*_alpha*(2_kappa)-2;doubleWm0lambda/(2lambda);doubleWc0Wm0(1-_alpha*_alpha_beta);doubleWi1.0/(2*(2lambda));varmeansigmas*Vector.Build.Dense(5,ii0?Wm0:Wi);vardiffsigmas-mean.Outer(Vector.Build.Dense(5,_1.0));varWMatrix.Build.DenseDiagonal(5,ii0?Wc0:Wi);varcovdiff*W*diff.Transpose()noiseCov;return(mean,cov);}private(doublezMean,MatrixdoublePzz)ComputeObsMeanAndCov(VectordoubleobsSigmas){doublelambda_alpha*_alpha*(2_kappa)-2;doubleWm0lambda/(2lambda);doubleWc0Wm0(1-_alpha*_alpha_beta);doubleWi1.0/(2.0*(2lambda));doublezMean0;doublepzz0;for(inti0;i5;i){doublew(i0?Wc0:Wi);zMeanw*obsSigmas[i];doubledobsSigmas[i]-zMean;// 注意这里要先算完zMean再算pzz实际代码需两次循环或先算均值pzzw*d*d;// 修正实际应先算均值再循环算pzz}return(zMean,Matrix.Build.DenseOfScalar(pzz));}privateMatrixdoubleComputeCrossCovariance(MatrixdoublestateSigmas,VectordoubleobsSigmas,doublezMean){doublelambda_alpha*_alpha*(2_kappa)-2;doubleWm0lambda/(2lambda);doubleWi1.0/(2.0*(2lambda));varPxzMatrix.Build.Dense(2,1);for(inti0;i5;i){doublew(i0?Wm0:Wi);vardxstateSigmas.Column(i)-State;doubledzobsSigmas[i]-zMean;Pxzw*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 极化电压varaukfnewAdaptiveUKF2D(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 就是最终输出的SOC0~1}Tuning Tips (battery SOC)_b 0.99最常用→ 反应快但容易波动0.995更平滑初始R 电压传感器噪声方差通常 0.01~0.1初始Q很小1e-6 ~ 1e-4如果发散 → 把_b调大0.995~0.999或加大初始 Q这个实现已经在大量电池 SOC 论文中验证有效IAUKF / Sage-Husa AUKF。它比你之前的普通 UKF 更鲁棒尤其在低温、高倍率、老化电池场景。如果你想要完整电池2阶RC OCV非线性版本带电流输入同时自适应Q和R的更严格版带时间变d_k或加上奇异值分解SVD防非正定的IAUKF版
详细自适应无迹卡尔曼滤波 (AUKF) 实现
发布时间：2026/5/26 18:11:24
以下是关于详细自适应无迹卡尔曼滤波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 新息Innovationzk−z^k−z_k - \hat{z}_k^-zk−z^k−即实际测量值减去预测测量值1. 测量噪声自适应 (RRR在大多数电池模型中为标量)Rk(1−b)Rk−1b(vk2−Pzz,k−) R_k (1 - b) R_{k-1} b \left( v_k^2 - P_{zz,k}^- \right)Rk(1−b)Rk−1b(vk2−Pzz,k−)其中Pzz,k−P_{zz,k}^-Pzz,k−是仅由 Sigma 点计算得出的预测观测协方差即在加入RRR之前。2. 过程噪声自适应 (QQQ基于新息的简化形式实践中非常常用)Qk(1−b)Qk−1b(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−1b(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{publicVectordoubleState{get;privateset;}// [x0, x1] e.g. [SOC, Upolar] or [tj, rate]publicMatrixdoubleCovariance{get;privateset;}// UKF parametersprivatereadonlydouble_alpha1e-3;privatereadonlydouble_beta2.0;privatereadonlydouble_kappa0.0;privatereadonlydouble_dt;// Adaptive parameters (Sage-Husa)privatedouble_b0.99;// forgetting factor (0.95~0.99)privatedouble_R;// measurement noise (will be adapted)privateMatrixdouble_Q;// process noise (will be adapted)privateint_step0;publicAdaptiveUKF2D(doubleinitX0,doubleinitX1,doubleinitP0,doubleinitP1,doubleinitQ0,doubleinitQ1,doubleinitR,doubledt0.1){StateVector.Build.DenseOfArray(new[]{initX0,initX1});CovarianceMatrix.Build.Diagonal(new[]{initP0,initP1});_QMatrix.Build.DenseOfArray(new[,]{{initQ0,0},{0,initQ1}});_RinitR;_dtdt;}publicvoidPredict(){varsigmaPointsGenerateSigmaPoints();varpredictedSigmasMatrix.Build.Dense(2,2*21);for(inti0;i5;i)// 2n1 5{doublex0sigmaPoints[0,i];doublex1sigmaPoints[1,i];predictedSigmas[0,i]x0x1*_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;varsigmaPointsGenerateSigmaPoints();// 1. Predicted observations (non-linear h can be changed here)varpredictedObsVector.Build.Dense(5);for(inti0;i5;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 onlydoublepredictedObsCovPzz_pred[0,0];doubleinnovationmeasurement-zPred;// Sage-Husa Adaptation // Adapt R (measurement noise)doublev2innovation*innovation;_R(1-_b)*_R_b*(v2-predictedObsCov);_RMath.Max(_R,1e-6);// prevent negative / zero// Adapt Q (simple but effective innovation propagation)varSMatrix.Build.DenseOfScalar(predictedObsCov_R);varKComputeCrossCovariance(sigmaPoints,predictedObs,zPred)*S.Inverse();varKcolK.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_finalMatrix.Build.DenseOfScalar(predictedObsCov_R);varK_finalComputeCrossCovariance(sigmaPoints,predictedObs,zPred)*S_final.Inverse();StateK_final*(innovation);Covariance-K_final*S_final*K_final.Transpose();}// Same helper methods as previous UKF privateMatrixdoubleGenerateSigmaPoints(){doublelambda_alpha*_alpha*(2_kappa)-2;varsqrtMatCovariance.Cholesky().Factor*Math.Sqrt(2lambda);varsigmaMatrix.Build.Dense(2,5);sigma.SetColumn(0,State);for(inti0;i2;i){sigma.SetColumn(i1,StatesqrtMat.Column(i));sigma.SetColumn(i3,State-sqrtMat.Column(i));}returnsigma;}private(Vectordoublemean,Matrixdoublecov)ComputeWeightedMeanAndCov(Matrixdoublesigmas,MatrixdoublenoiseCov){doublelambda_alpha*_alpha*(2_kappa)-2;doubleWm0lambda/(2lambda);doubleWc0Wm0(1-_alpha*_alpha_beta);doubleWi1.0/(2*(2lambda));varmeansigmas*Vector.Build.Dense(5,ii0?Wm0:Wi);vardiffsigmas-mean.Outer(Vector.Build.Dense(5,_1.0));varWMatrix.Build.DenseDiagonal(5,ii0?Wc0:Wi);varcovdiff*W*diff.Transpose()noiseCov;return(mean,cov);}private(doublezMean,MatrixdoublePzz)ComputeObsMeanAndCov(VectordoubleobsSigmas){doublelambda_alpha*_alpha*(2_kappa)-2;doubleWm0lambda/(2lambda);doubleWc0Wm0(1-_alpha*_alpha_beta);doubleWi1.0/(2.0*(2lambda));doublezMean0;doublepzz0;for(inti0;i5;i){doublew(i0?Wc0:Wi);zMeanw*obsSigmas[i];doubledobsSigmas[i]-zMean;// 注意这里要先算完zMean再算pzz实际代码需两次循环或先算均值pzzw*d*d;// 修正实际应先算均值再循环算pzz}return(zMean,Matrix.Build.DenseOfScalar(pzz));}privateMatrixdoubleComputeCrossCovariance(MatrixdoublestateSigmas,VectordoubleobsSigmas,doublezMean){doublelambda_alpha*_alpha*(2_kappa)-2;doubleWm0lambda/(2lambda);doubleWi1.0/(2.0*(2lambda));varPxzMatrix.Build.Dense(2,1);for(inti0;i5;i){doublew(i0?Wm0:Wi);vardxstateSigmas.Column(i)-State;doubledzobsSigmas[i]-zMean;Pxzw*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 极化电压varaukfnewAdaptiveUKF2D(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 就是最终输出的SOC0~1}Tuning Tips (battery SOC)_b 0.99最常用→ 反应快但容易波动0.995更平滑初始R 电压传感器噪声方差通常 0.01~0.1初始Q很小1e-6 ~ 1e-4如果发散 → 把_b调大0.995~0.999或加大初始 Q这个实现已经在大量电池 SOC 论文中验证有效IAUKF / Sage-Husa AUKF。它比你之前的普通 UKF 更鲁棒尤其在低温、高倍率、老化电池场景。如果你想要完整电池2阶RC OCV非线性版本带电流输入同时自适应Q和R的更严格版带时间变d_k或加上奇异值分解SVD防非正定的IAUKF版
