171 lines
3.5 KiB
Go
171 lines
3.5 KiB
Go
// diff.go
|
|
package diff
|
|
|
|
import (
|
|
"fmt"
|
|
"math"
|
|
"strconv"
|
|
"strings"
|
|
)
|
|
|
|
type KalmanVarDiff struct {
|
|
Kf *KalmanFilter
|
|
|
|
StartDiff float64
|
|
MinDiff float64
|
|
MaxDiff float64
|
|
|
|
TargetInterval float64
|
|
|
|
MhsEst float64
|
|
DiffEst float64
|
|
}
|
|
|
|
type KalmanFilter struct {
|
|
X float64 // 估计的状态
|
|
P float64 // 状态协方差
|
|
F float64 // 状态转移矩阵
|
|
H float64 // 观测矩阵
|
|
Q float64 // 过程噪声协方差
|
|
R float64 // 观测噪声协方差
|
|
K float64 // 卡尔曼增益
|
|
}
|
|
|
|
func NewKalmanFilter(init_mhs float64, init_state_p float64) *KalmanFilter {
|
|
x := init_mhs
|
|
p := init_state_p
|
|
f := 1.0
|
|
h := 1.0
|
|
q := 0.01
|
|
r := init_mhs * 3
|
|
//r := 1.0
|
|
return &KalmanFilter{
|
|
X: x,
|
|
P: p,
|
|
F: f,
|
|
H: h,
|
|
Q: q,
|
|
R: r,
|
|
}
|
|
/*
|
|
return &KalmanFilter{
|
|
X: 1.0,
|
|
P: 1.0,
|
|
F: 1.0,
|
|
H: 1.0,
|
|
Q: 0.1,
|
|
R: 1.0,
|
|
}*/
|
|
}
|
|
|
|
func (kf *KalmanFilter) Update(measurement float64) (float64, float64) {
|
|
kf.R = measurement * 2
|
|
|
|
// 预测
|
|
p := kf.X*kf.Q + kf.P
|
|
|
|
// 计算卡尔曼增益
|
|
kf.K = p / (p + kf.R + 1)
|
|
|
|
// 更新状态估计
|
|
if measurement >= kf.X {
|
|
kf.X = kf.X + kf.K*(measurement-kf.X)
|
|
} else {
|
|
kf.X = kf.X - kf.K*(kf.X-measurement)
|
|
}
|
|
|
|
// 更新协方差矩阵
|
|
kf.P = (1 - kf.K) * p
|
|
|
|
// 自适应调整过程噪声和观测噪声
|
|
//kf.adapt()
|
|
|
|
return kf.X, kf.P
|
|
}
|
|
|
|
func (kf *KalmanFilter) adapt() {
|
|
// 自适应调整参数
|
|
if kf.P > 10.0 {
|
|
kf.Q *= 1.1 // 增加过程噪声
|
|
} else {
|
|
kf.Q *= 0.9 // 减少过程噪声
|
|
}
|
|
if kf.K > 0.5 {
|
|
kf.R *= 1.1 // 增加观测噪声
|
|
} else {
|
|
kf.R *= 0.9 // 减少观测噪声
|
|
}
|
|
}
|
|
|
|
func (kd *KalmanVarDiff) Init(startDiff float64, minDiff float64, maxDiff float64, targetTime float64) {
|
|
kd.StartDiff = startDiff
|
|
kd.MinDiff = minDiff
|
|
kd.MaxDiff = maxDiff
|
|
kd.TargetInterval = targetTime
|
|
kd.DiffEst = startDiff
|
|
kd.MhsEst = startDiff / targetTime
|
|
kd.Kf = NewKalmanFilter(startDiff/targetTime, 1.0)
|
|
}
|
|
|
|
func (kd *KalmanVarDiff) DeInit() {
|
|
|
|
}
|
|
|
|
// 提取科学计数法有效数字（整数部分和一位小数）及指数，并合并为新的浮点数
|
|
func extractAndCombine(num float64) float64 {
|
|
// 将浮点数格式化为科学计数法
|
|
scientificStr := fmt.Sprintf("%.10e", num)
|
|
|
|
// 分离小数部分和指数部分
|
|
parts := strings.Split(scientificStr, "e")
|
|
if len(parts) != 2 {
|
|
fmt.Println("Error: unexpected scientific notation format")
|
|
return 0
|
|
}
|
|
|
|
// 处理小数部分
|
|
decimalPart := parts[0]
|
|
exponentPart := parts[1]
|
|
|
|
// 去除小数部分前的 "0."
|
|
decimalPart = strings.TrimPrefix(decimalPart, "0.")
|
|
|
|
// 提取整数部分和一位小数
|
|
decimalParts := strings.Split(decimalPart, ".")
|
|
if len(decimalParts) < 2 {
|
|
decimalPart = decimalParts[0] + ".0" // 没有小数部分时，添加 ".0"
|
|
} else {
|
|
decimalPart = decimalParts[0] + "." + decimalParts[1][:1] // 只取一位小数
|
|
//decimalPart = decimalParts[0]
|
|
}
|
|
|
|
// 将指数部分转换为整数
|
|
exponent, err := strconv.Atoi(exponentPart)
|
|
if err != nil {
|
|
fmt.Println("Error parsing exponent:", err)
|
|
return 0
|
|
}
|
|
|
|
// 计算新的浮点数
|
|
newNumber := (func() float64 {
|
|
digit, err := strconv.ParseFloat(decimalPart, 64)
|
|
if err != nil {
|
|
fmt.Println("Error parsing decimal part:", err)
|
|
return 0
|
|
}
|
|
return digit * math.Pow(10, float64(exponent))
|
|
})()
|
|
|
|
return newNumber
|
|
}
|
|
|
|
func (kd *KalmanVarDiff) Handler(diff float64, interval float64) (float64, float64) {
|
|
//newx, newp := kd.Kf.Update(kd.DiffEst / interval)
|
|
newx, newp := kd.Kf.Update(diff / interval)
|
|
kd.MhsEst = newx
|
|
newdiff := newx * kd.TargetInterval
|
|
kd.DiffEst = newdiff
|
|
newdiff2 := extractAndCombine(newdiff)
|
|
return newdiff2, newp
|
|
}
|