'--------------------- 可调参数 --------------------------- Public Alpha As Double ' 位置修正系数 Public Beta As Double ' 速度修正系数 Public Gamma As Double ' 加速度修正系数 Public HistoryLen As Integer
'--------------------- 内部状态 --------------------------- Private x As Double ' 滤波值 Private v As Double ' 速度 Private a As Double ' 加速度 Private ReadOnly residuals As New Queue(Of Double)()
'---------------------------------------------------------- ' 构造函数(默认保守值) '---------------------------------------------------------- Public Sub New(Optional alpha As Double = 0.001, Optional beta As Double = 0.0001, Optional gamma As Double = 0.00001, Optional historyLen As Integer = 50) Me.Alpha = alpha Me.Beta = beta Me.Gamma = gamma Me.HistoryLen = historyLen End Sub
'---------------------------------------------------------- ' 每来一个新样本调用一次,返回滤波后的值 '---------------------------------------------------------- Public Function Update(raw As Double) As Double ' 1. 预测 Dim xPred As Double = x + v + 0.5 * a Dim vPred As Double = v + a Dim aPred As Double = a
' 2. 残差 Dim residual As Double = raw - xPred
' 3. 动态阈值(防零 / 防负 / 防空) Dim threshold As Double = GetDynamicThreshold()
' 4. 限幅:残差被限制在 [-threshold, +threshold] Dim useResidual As Double = Math.Max(-threshold, Math.Min(threshold, residual))
' 5. 更新状态 x = xPred + Alpha * useResidual v = vPred + Beta * useResidual a = aPred + Gamma * useResidual
' 6. 保存残差 residuals.Enqueue(residual) If residuals.Count > HistoryLen Then residuals.Dequeue()
Return x End Function
'---------------------------------------------------------- ' 计算 3σ 阈值(防零、防负、防空队列) '---------------------------------------------------------- Private Function GetDynamicThreshold() As Double Const MIN_THRESHOLD As Double = 0.001 Const MIN_SAMPLES As Integer = 3
If residuals.Count < MIN_SAMPLES Then Return 1.0
Dim arr = residuals.ToArray() Dim avg As Double = arr.Average() Dim var As Double = arr.Select(Function(val) (val - avg) ^ 2).Average()
If var < 0 Then var = 0 ' 防负 Dim std As Double = Math.Sqrt(var) If std = 0 Then std = 0.001 ' 防零
Return Math.Max(MIN_THRESHOLD, 3.0 * std) End Function