这是两个正弦函数相加生成新函数,根据我提供的代码,将梯形积分得到的位移displacement_corrected和displacement_GPS_noisy通过用卡尔曼滤波进行融合得到位移图如下:
融合之前因为梯形积分和GPS测得位移的采样频率不同,其样本点不同,要先解决这个问题
1.写出解决采样频率不同的代码
2.写出使用卡尔曼滤波融合的代码
3.所得结果画图,最后位移图与我发的一致
4.可以进行其他操作
import numpy as np
import matplotlib.pyplot as plt
from scipy import signal
from scipy.integrate import cumulative_trapezoid
from scipy.interpolate import interp1d
from scipy.signal import butter, filtfilt
# 定义时间范围
t = np.arange(0, 10 * np.pi, 0.01) # 时间序列
def generate_sine_waves(t, A1, ω1, φ1, A2, ω2, φ2):
"""
生成两个正弦波并叠加。
参数:
t : 时间序列
A1 : 第一个正弦波的振幅
ω1 : 第一个正弦波的频率
φ1 : 第一个正弦波的相位
A2 : 第二个正弦波的振幅
ω2 : 第二个正弦波的频率
φ2 : 第二个正弦波的相位
返回:
y_sum : 叠加后的正弦波信号
"""
# 正弦波1:A1 * sin(ω1 * t + φ1)
y1 = A1 * np.sin(ω1 * t + φ1)
# 正弦波2:A2 * sin(ω2 * t + φ2)
y2 = A2 * np.sin(ω2 * t + φ2)
# 叠加两个正弦波
y_sum = y1 + y2
return y_sum
# 正弦波1:A1 * sin(ω1 * t + φ1)
A1 = 1.0 # 振幅
ω1 = 2 * np.pi # 频率 (1Hz)
φ1 = 0 # 相位
y1 = A1 * np.sin(ω1 * t + φ1)
# 正弦波2:A2 * sin(ω2 * t + φ2)
A2 = 0.5 # 振幅
ω2 = 2 * np.pi * 2 # 频率 (2Hz)
φ2 = np.pi / 4 # 相位
y2 = A2 * np.sin(ω2 * t + φ2)
# 叠加两个正弦波
y_sum = generate_sine_waves(t, A1, ω1, φ1, A2, ω2, φ2)
# 计算加速度(两次导数)
y_acceleration = np.gradient(np.gradient(y_sum, t), t)
# 生成高斯白噪声
def add_noise(y, level):
"""
给信号添加高斯白噪声
输入:原始信号 y(数组),噪声水平 level(百分比)
输出:添加噪声后的信号(行向量)
"""
# 采样点个数
ttp = len(y)
# 计算信号的平方和
sumy = np.sum(y ** 2)
# 计算信号的均方根(RMS)
rmsy = np.sqrt(sumy / ttp)
# 生成高斯白噪声,并进行标准化(均值为 0,标准差为 1)
RdNoise1 = np.random.randn(ttp)
RdNoise1 = RdNoise1 / np.std(RdNoise1)
# 将噪声添加到原始信号中
f = y + (level / 100) * rmsy * RdNoise1
return f
noise_level =5 #
y_acceleration= add_noise(y_acceleration, noise_level)
# 时间步长
dt = t[1] - t[0]
# 梯形法积分函数
def trapezoidal_integration(y, dt):
"""使用梯形法对信号积分"""
integral = np.cumsum((y[1:] + y[:-1]) / 2) * dt
# 补齐初始点,使结果与输入长度一致
return np.insert(integral, 0, 0)
# 计算速度(对加速度进行积分)
velocity_trapz = trapezoidal_integration(y_acceleration, dt)
# 设置初始速度为1
initial_velocity = 1
velocity_trapz += (initial_velocity - velocity_trapz[0])
poly_order = 2 # 多项式阶数
coeffs = np.polyfit(t, velocity_trapz, poly_order) # 拟合多项式系数
velocity_trend = np.polyval(coeffs, t) # 计算拟合趋势
# 校正后的位移:去除拟合出的趋势部分
velocity_corrected = velocity_trapz - velocity_trend
# 计算位移(对速度进行积分)
displacement_trapz = trapezoidal_integration(velocity_corrected, dt)
poly_order = 2 # 多项式阶数
coeffs = np.polyfit(t, displacement_trapz, poly_order) # 拟合多项式系数
displacement_trend = np.polyval(coeffs, t) # 计算拟合趋势
# 校正后的位移:去除拟合出的趋势部分
displacement_corrected = displacement_trapz - displacement_trend
t_gps= t[::100] # 每隔100个点取一个,步长为1
noise_level = 5 # 噪声水平
displacement_GPS = generate_sine_waves(t_gps, A1, ω1, φ1, A2, ω2, φ2)
displacement_GPS_noisy = add_noise(displacement_GPS, noise_level) # 加入噪声的GPS信号