GPS关闭消息和连续位置更新颤动

import numpy as np

class KalmanFilter:
    def __init__(self, process_variance, measurement_variance):
        self.process_variance = process_variance
        self.measurement_variance = measurement_variance
        self.posteri_estimate = 0.0
        self.posteri_error_estimate = 1.0

    def update(self, measurement):
        priori_estimate = self.posteri_estimate
        priori_error_estimate = self.posteri_error_estimate + self.process_variance

        blending_factor = priori_error_estimate / (priori_error_estimate + self.measurement_variance)
        self.posteri_estimate = priori_estimate + blending_factor * (measurement - priori_estimate)
        self.posteri_error_estimate = (1 - blending_factor) * priori_error_estimate

        return self.posteri_estimate

# 示例使用
measurements = [10.1, 10.3, 10.2, 10.5, 10.4]  # 模拟的位置数据
kf = KalmanFilter(process_variance=0.01, measurement_variance=0.1)

smoothed_positions = []
for measurement in measurements:
    smoothed_position = kf.update(measurement)
    smoothed_positions.append(smoothed_position)

print("原始位置数据:", measurements)
print("平滑后的位置数据:", smoothed_positions)