python 数据处理之卡尔曼，均值，中位数

import numpy as np
import matplotlib.pyplot as plt
from filterpy.kalman import KalmanFilter
import matplotlib.pyplot as plt

# 设置中文字体
plt.rcParams['font.sans-serif'] = ['SimHei']  # 使用黑体
plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题



def moving_average(noisy_measurement, window_size):
    return np.convolve(noisy_measurement, np.ones(window_size)/window_size, mode='valid')


def sliding_window_median(data, window_size):
    medians = []
    
    for i in range(len(data) - window_size + 1):
        window = data[i:i+window_size]
        median = np.median(window)
        medians.append(median)
    
    return medians


# 从文本文件读取数据
data = np.loadtxt('/home/local/EUROPRO/guoliang.wang/Downloads/ufd_median/苏州机器/CTM 探索/mnt/udisk/map/ufd_data.txt')  # 请替换成你的文件名

# 获取测量值
noisy_measurement = data[:, 0]
th_voule = data[:,1]
# 中值滤波
window_size = 32
median_filtered_values = sliding_window_median(noisy_measurement, window_size)

moving_avg_values = moving_average(noisy_measurement, window_size)

median_avg_value = moving_average(median_filtered_values, window_size)

avg_median = sliding_window_median(moving_avg_values, window_size)



# 生成时间序列号
time_series = np.arange(len(median_filtered_values))

# # 初始化卡尔曼滤波器
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.F = np.array([[1, 1], [0, 1]])  # 状态转移矩阵
kf.H = np.array([[1, 0]])          # 观测矩阵
kf.Q *= 0.01                       # 过程噪声协方差矩阵
kf.R = 0.1                         # 观测噪声方差
kf.x = np.array([0, 0])            # 初始状态估计
kf.P *= 1                          # 初始协方差矩阵

# 卡尔曼滤波
filtered_state_means = []
for measurement in median_filtered_values:
    kf.predict()          # 预测步骤
    kf.update(measurement) # 更新步骤
    filtered_state_means.append(kf.x[0])  # 保存滤波后的状态

# 绘制结果

x = np.arange(len(noisy_measurement))
plt.figure(1)
plt.subplot(2,3,1)
plt.scatter(x,noisy_measurement, label='真实位置', marker='.', color='red')
plt.plot(th_voule)
plt.title('test_raw_data')
plt.grid(True)

plt.subplot(2,3,2)
plt.scatter(time_series, filtered_state_means, label='滤波后的状态', marker='.', color='blue')
plt.title('median_kalman')
plt.plot(th_voule)
plt.legend()
plt.grid(True)

x = np.arange(len(moving_avg_values))
plt.subplot(2,3,3)
plt.scatter(x,moving_avg_values, label='真实位置', marker='.', color='green')
plt.title('avg')
plt.grid(True)
plt.plot(th_voule)
x = np.arange(len(median_filtered_values))


plt.subplot(2,3,4)
plt.scatter(x,median_filtered_values, label='真实位置', marker='.', color='black')
plt.title('median')
plt.grid(True)
plt.plot(th_voule)
x = np.arange(len(median_avg_value))


plt.subplot(2,3,5)
plt.scatter(x,median_avg_value, label='真实位置', marker='.', color='black')
plt.title('median_avg')
plt.grid(True)
plt.plot(th_voule)

plt.subplot(2,3,6)
plt.scatter(x,avg_median, label='真实位置', marker='.', color='black')
plt.title('avg_med')
plt.grid(True)
plt.plot(th_voule)
plt.suptitle('17# BJ 1st Floor', fontsize=16)

plt.show()