一、算法原理概述
-
小波变换（Wavelet Transform）
- 通过多尺度分解将信号分为高频（细节）和低频（近似）部分，高频通常包含噪声，低频保留主体信息。
- 使用Haar小波（计算高效）进行快速分解与重构：
// Haar小波分解公式 approx = (x[2i] + x[2i+1]) / 2.0; detail = (x[2i] - x[2i+1]) / 2.0;
-
卡尔曼滤波（Kalman Filter）
- 预测-更新两阶段递归估计[citation:5][citation:6]：
- 预测：基于状态方程预估当前状态和误差协方差
Xpred=A⋅Xprev+B⋅uPpred=A⋅Pprev⋅AT+QXpred=A⋅Xprev+B⋅uPpred=A⋅Pprev⋅AT+Q
- 更新：结合观测值修正预测值，计算卡尔曼增益
KK=Ppred⋅HT/(H⋅Ppred⋅HT+R)Xnew=Xpred+K⋅(z−H⋅Xpred)Pnew=(I−K⋅H)⋅PpredK=Ppred⋅HT/(H⋅Ppred⋅HT+R)Xnew=Xpred+K⋅(z−H⋅Xpred)Pnew=(I−K⋅H)⋅Ppred
- 预测：基于状态方程预估当前状态和误差协方差
- 预测-更新两阶段递归估计[citation:5][citation:6]：
-
小波卡尔曼融合
- 先对原始信号小波分解，对不同频段独立应用卡尔曼滤波，最后重构信号
- 高频部分使用更高测量噪声协方差
R（抑制噪声），低频部分使用更低R（保留主体）。
二、Rust实现代码
cargo.toml
[package]
name = "wavelet-kalman-filter"
version = "0.1.0"
edition = "2021"
[dependencies]
plotters = "0.3.5"
rand = "0.8.5"
log = "0.4.17"
simple_logger = "5.0.0"
main.rs
use log::{info};
use log::LevelFilter;
use simple_logger::SimpleLogger;
// 初始化日志记录，将日志保存到文件
fn init_logger() {
SimpleLogger::new()
.with_level(LevelFilter::Info)
.with_utc_timestamps()
.init()
.expect("Failed to initialize logger");
info!("日志系统初始化完成");
}
// 主结构体
pub struct WaveletKalmanFilter {
pub a: f64, // 状态转移系数（如匀速运动时A=1）
pub q: f64, // 过程噪声协方差
pub r_low: f64, // 低频观测噪声协方差
pub r_high: f64, // 高频观测噪声协方差
pub state: f64, // 当前状态估计
pub cov: f64, // 当前误差协方差
}
// 一维Haar小波分解（返回低频近似 + 高频细节）
pub fn haar_decompose(signal: &[f64]) -> (Vec<f64>, Vec<f64>) {
info!("开始Haar小波分解，输入信号长度: {}", signal.len());
// 对信号进行对称延拓，以缓解小波分析的边界问题
fn symmetric_extend(signal: &[f64]) -> Vec<f64> {
if signal.len() <= 1 {
info!("信号长度小于等于1，直接返回原信号");
return signal.to_vec();
}
let mut extended = Vec::new();
// 左侧对称部分
for i in (1..=signal.len() - 1).rev() {
extended.push(signal[i]);
}
// 原始信号部分
extended.extend_from_slice(signal);
// 右侧对称部分
for i in 1..signal.len() {
extended.push(signal[signal.len() - 1 - i]);
}
info!("对称延拓后信号长度: {} 信号内容: {:?}", extended.len(), extended);
extended
}
// 对延拓后的信号进行Haar小波分解
pub fn haar_decompose(signal: &[f64]) -> (Vec<f64>, Vec<f64>) {
let extended = symmetric_extend(signal);
let mut approx = Vec::new();
let mut detail = Vec::new();
for i in (0..extended.len()).step_by(2) {
let sum = extended[i] + extended.get(i + 1).unwrap_or(&0.0);
let diff = extended[i] - extended.get(i + 1).unwrap_or(&0.0);
approx.push(sum / 2.0);
detail.push(diff / 2.0);
}
// 截取与原始信号分解后相同长度的结果
let half_len = (signal.len() + 1) / 2;
let approx = approx[extended.len() / 4..extended.len() / 4 + half_len].to_vec();
let detail = detail[extended.len() / 4..extended.len() / 4 + half_len].to_vec();
info!(
"延拓后Haar分解完成，近似分量长度: {}, 细节分量长度: {}",
approx.len(),
detail.len()
);
(approx, detail)
}
let mut approx = Vec::new();
let mut detail = Vec::new();
for i in (0..signal.len()).step_by(2) {
let sum = signal[i] + signal.get(i + 1).unwrap_or(&0.0);
let diff = signal[i] - signal.get(i + 1).unwrap_or(&0.0);
approx.push(sum / 2.0);
detail.push(diff / 2.0);
}
info!(
"直接Haar分解完成，近似分量长度: {}, 细节分量长度: {}",
approx.len(),
detail.len()
);
(approx, detail)
}
// 一维Haar小波重构
pub fn haar_recompose(approx: &[f64], detail: &[f64]) -> Vec<f64> {
info!(
"开始Haar小波重构，近似分量长度: {}, 细节分量长度: {}",
approx.len(),
detail.len()
);
let mut output = Vec::with_capacity(approx.len() * 2);
for i in 0..approx.len() {
output.push(approx[i] + detail[i]);
output.push(approx[i] - detail[i]);
}
info!("Haar小波重构完成，输出信号长度: {}", output.len());
output
}
impl WaveletKalmanFilter {
// 初始化滤波器
pub fn new(a: f64, q: f64, r_low: f64, r_high: f64) -> Self {
info!(
"创建新的WaveletKalmanFilter，a: {}, q: {}, r_low: {}, r_high: {}",
a, q, r_low, r_high
);
WaveletKalmanFilter {
a,
q,
r_low,
r_high,
state: 0.0,
cov: 1.0, // 初始协方差设为较大值（表示不确定性高）
}
}
// 单步预测
pub fn predict(&mut self) {
info!("执行单步预测前，状态: {}, 协方差: {}", self.state, self.cov);
self.state = self.a * self.state; // X_pred = A * X_prev
self.cov = self.a * self.cov * self.a + self.q; // P_pred = A*P*A^T + Q
info!("执行单步预测后，状态: {}, 协方差: {}", self.state, self.cov);
}
// 单步更新（根据频段选择R）
pub fn update(&mut self, measurement: f64, is_low_freq: bool) {
// info!(
// "执行单步更新前，测量值: {}, 是否低频: {}",
// measurement, is_low_freq
// );
let r = if is_low_freq { self.r_low } else { self.r_high };
let k = self.cov / (self.cov + r); // 卡尔曼增益K
self.state += k * (measurement - self.state); // X_new = X_pred + K*(z - H*X_pred)
self.cov = (1.0 - k) * self.cov; // P_new = (I - K*H)*P_pred
info!("执行单步更新后，状态: {}, 协方差: {}", self.state, self.cov);
}
}
pub fn wavelet_kalman_filter(
signal: &[f64],
levels: usize, // 小波分解层数
a: f64, // 状态转移系数
q: f64, // 过程噪声
r_low: f64, // 低频观测噪声
r_high: f64, // 高频观测噪声
) -> Vec<f64> {
info!(
"开始小波卡尔曼滤波，输入信号长度: {}, 分解层数: {}",
signal.len(),
levels
);
// 1. 递归小波分解（多尺度）
let mut approx = signal.to_vec();
let mut details = Vec::new();
for level in 0..levels {
info!("第 {} 层小波分解", level + 1);
let (a, d) = haar_decompose(&approx);
details.push(d);
approx = a;
}
// 2. 对各频段独立应用卡尔曼滤波
let mut kalman = WaveletKalmanFilter::new(a, q, r_low, r_high);
kalman.state = approx[0]; // 初始化状态
// 低频滤波（R较小，信任观测值）
info!("开始低频滤波，近似分量长度: {}", approx.len());
for i in 0..approx.len() {
info!("低频滤波第 {} 步", i + 1);
kalman.predict();
kalman.update(approx[i], true); // 标记为低频
approx[i] = kalman.state;
}
// 高频滤波（R较大，抑制噪声）
info!("开始高频滤波，细节分量数量: {}", details.len());
for (level, d) in details.iter_mut().enumerate() {
info!("第 {} 层细节分量滤波，长度: {}", level + 1, d.len());
kalman = WaveletKalmanFilter::new(a, q, r_low, r_high); // 重置滤波器
kalman.state = d[0];
for i in 0..d.len() {
info!("第 {} 层细节分量第 {} 步滤波", level + 1, i + 1);
kalman.predict();
kalman.update(d[i], false); // 标记为高频
d[i] = kalman.state;
}
}
// 3. 小波重构
info!("开始小波重构，分解层数: {}", levels);
for _ in 0..levels {
approx = haar_recompose(&approx, &details.pop().unwrap());
}
info!("小波卡尔曼滤波完成，输出信号长度: {}", approx.len());
approx
}
// 绘图函数实现
fn draw_signals(
clean: &[f64],
noisy: &[f64],
filtered: &[f64],
) -> Result<(), Box<dyn std::error::Error>> {
info!(
"开始绘制信号图，原始信号长度: {}, 含噪信号长度: {}, 滤波信号长度: {}",
clean.len(),
noisy.len(),
filtered.len()
);
use plotters::prelude::*;
let root = BitMapBackend::new("signals.png", (1024, 768)).into_drawing_area();
root.fill(&WHITE)?;
let mut chart = ChartBuilder::on(&root)
.caption("信号处理结果", ("sans-serif", 50).into_font())
.margin(10)
.x_label_area_size(30)
.y_label_area_size(30)
.build_cartesian_2d(0f64..clean.len() as f64, -2f64..2f64)?;
chart.configure_mesh().draw()?;
chart
.draw_series(LineSeries::new(
clean.iter().enumerate().map(|(i, &y)| (i as f64, y)),
&RED,
))?
.label("原始信号")
.legend(|(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &RED));
chart
.draw_series(LineSeries::new(
noisy.iter().enumerate().map(|(i, &y)| (i as f64, y)),
&BLUE,
))?
.label("含噪信号")
.legend(|(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &BLUE));
chart
.draw_series(LineSeries::new(
filtered.iter().enumerate().map(|(i, &y)| (i as f64, y)),
&GREEN,
))?
.label("滤波信号")
.legend(|(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &GREEN));
chart
.configure_series_labels()
.background_style(&WHITE.mix(0.8))
.border_style(&BLACK)
.draw()?;
info!("信号图绘制完成，保存至 signals.png");
Ok(())
}
// 测试函数：添加高斯噪声的信号滤波
fn test_noisy_sine() {
info!("开始测试含噪正弦信号滤波");
let clean_signal: Vec<_> = (0..1000).map(|i| (i as f64 * 0.1).sin()).collect();
let noisy_signal: Vec<_> = clean_signal
.iter()
.map(|x| x + rand::random::<f64>())
.collect();
let filtered = wavelet_kalman_filter(&noisy_signal, 3, 1.0, 0.01, 0.1, 2.0);
// 绘图比较（使用plotters库）
draw_signals(&clean_signal, &noisy_signal, &filtered);
info!("含噪正弦信号滤波测试完成");
}
// 主函数
fn main() {
init_logger();
info!("程序启动");
test_noisy_sine();
info!("程序结束");
}
实时小波卡尔曼滤波
// 引入 nalgebra 库中的矩阵、向量和标量类型
use nalgebra::{DMatrix, DVector, Scalar};
// 引入标准库中的 π 常量
use std::f64::consts::PI;
// 引入标准库中的双端队列
use std::collections::VecDeque;
// 定义实时小波卡尔曼滤波器结构体
pub struct RealTimeWaveletKalmanFilter {
// 当前状态向量
state: DVector<f64>,
// 状态协方差矩阵
covariance: DMatrix<f64>,
// 状态转移矩阵
transition: DMatrix<f64>,
// 观测矩阵
observation: DMatrix<f64>,
// 过程噪声协方差矩阵
process_noise: DMatrix<f64>,
// 测量噪声协方差矩阵
measurement_noise: DMatrix<f64>,
// 小波类型
wavelet_type: WaveletType,
// 小波分解的层数
decomposition_level: usize,
// 小波系数阈值
threshold: f64,
// 数据缓冲区，用于存储待处理的数据点
buffer: VecDeque<f64>,
// 缓冲区的最小大小，取决于小波类型
min_buffer_size: usize,
// 缓冲区的大小
buffer_size: usize,
// 已处理的数据点数量
processed_count: usize,
// 存储各层近似系数的缓冲区
approx_buffers: Vec<Vec<f64>>,
// 存储各层细节系数的缓冲区
detail_buffers: Vec<Vec<f64>>,
}
// 定义小波类型枚举，支持 Haar 和 Daubechies4 两种小波
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum WaveletType {
Haar,
Daubechies4,
}
// 实现实时小波卡尔曼滤波器的方法
impl RealTimeWaveletKalmanFilter {
// 构造函数，初始化实时小波卡尔曼滤波器
pub fn new(
initial_state: DVector<f64>,
initial_covariance: DMatrix<f64>,
transition: DMatrix<f64>,
observation: DMatrix<f64>,
process_noise: DMatrix<f64>,
measurement_noise: DMatrix<f64>,
wavelet_type: WaveletType,
decomposition_level: usize,
threshold: f64,
) -> Self {
// 根据小波类型确定最小缓冲区大小
let min_buffer_size = match wavelet_type {
WaveletType::Haar => 2,
WaveletType::Daubechies4 => 4,
};
// 计算缓冲区大小，通过 2 的 decomposition_level 次幂得到
let buffer_size = 1 << decomposition_level;
// 初始化各层近似系数缓冲区
let approx_buffers = vec![vec![]; decomposition_level];
// 初始化各层细节系数缓冲区
let detail_buffers = vec![vec![]; decomposition_level];
// 返回初始化后的实时小波卡尔曼滤波器实例
RealTimeWaveletKalmanFilter {
state: initial_state,
covariance: initial_covariance,
transition,
observation,
process_noise,
measurement_noise,
wavelet_type,
decomposition_level,
threshold,
buffer: VecDeque::with_capacity(buffer_size * 2),
min_buffer_size,
buffer_size,
processed_count: 0,
approx_buffers,
detail_buffers,
}
}
// 添加一个新的数据点到滤波器，并尝试进行处理
pub fn add_data_point(&mut self, measurement: f64) -> Option<f64> {
// 将新的数据点添加到缓冲区
self.buffer.push_back(measurement);
// 检查缓冲区中的数据量是否达到最小缓冲区大小
if self.buffer.len() >= self.min_buffer_size {
// 尝试进行小波处理，得到去噪后的数据
if let Some(denoised) = self.process_wavelets() {
// 进行卡尔曼预测
self.predict();
// 进行卡尔曼更新
self.update(denoised);
// 返回更新后的状态向量的第一个元素
return Some(self.state[0]);
}
}
// 如果无法处理，返回 None
None
}
// 处理小波变换，包括分解、阈值处理和重构
fn process_wavelets(&mut self) -> Option<f64> {
// 计算当前处理的小波分解层数
let level = (self.processed_count % self.decomposition_level) + 1;
// 确保层数不超过最大分解层数
let level = level.min(self.decomposition_level);
// 获取数据块的大小
let block_size = self.buffer_size;
// 检查缓冲区中的数据量是否足够一个数据块
if self.buffer.len() < block_size {
return None;
}
// 从缓冲区中取出一个数据块
let mut block: Vec<f64> = self.buffer.drain(..block_size).collect();
// 初始化去噪后的数据块（此处变量未使用）
let _denoised_block: Vec<f64> = Vec::with_capacity(block_size);
// 进行小波分解
for _ in 0..level {
// 检查数据块的大小是否足够进行小波分解
if block.len() < self.min_buffer_size {
break;
}
// 根据小波类型进行分解，得到近似系数和细节系数
let (approx, detail) = match self.wavelet_type {
WaveletType::Haar => self.haar_decompose(&block),
WaveletType::Daubechies4 => self.daubechies4_decompose(&block),
};
// 将近似系数添加到对应的缓冲区
self.approx_buffers[level - 1].extend_from_slice(&approx);
// 将细节系数添加到对应的缓冲区
self.detail_buffers[level - 1].extend_from_slice(&detail);
// 更新数据块为近似系数，以便进行下一层分解
block = approx;
}
// 对各层的近似系数和细节系数进行阈值处理
for i in 0..level {
self.approx_buffers[i] = self.threshold_coefficients(self.approx_buffers[i].clone());
self.detail_buffers[i] = self.threshold_coefficients(self.detail_buffers[i].clone());
}
// 从最高层的近似系数开始重构
let mut reconstructed = self.approx_buffers[level - 1].clone();
// 逐层进行小波重构
for i in (0..level).rev() {
reconstructed = match self.wavelet_type {
WaveletType::Haar => self.haar_reconstruct(&reconstructed, &self.detail_buffers[i]),
WaveletType::Daubechies4 => self.daubechies4_reconstruct(&reconstructed, &self.detail_buffers[i]),
};
// 计算需要保留的系数数量
let keep_count = (self.buffer_size >> i).max(self.min_buffer_size);
// 获取近似系数缓冲区的长度
let approx_len = self.approx_buffers[i].len();
// 获取细节系数缓冲区的长度
let detail_len = self.detail_buffers[i].len();
// 复制近似系数缓冲区
let mut approx_copy = self.approx_buffers[i].clone();
// 复制细节系数缓冲区
let mut detail_copy = self.detail_buffers[i].clone();
// 保留近似系数缓冲区的最后 keep_count 个元素
self.approx_buffers[i] = approx_copy.split_off(approx_len.saturating_sub(keep_count));
// 保留细节系数缓冲区的最后 keep_count 个元素
self.detail_buffers[i] = detail_copy.split_off(detail_len.saturating_sub(keep_count));
}
// 如果重构后的数据存在，更新已处理数据点数量并返回最后一个元素
if let Some(last_value) = reconstructed.last() {
self.processed_count += 1;
return Some(*last_value);
}
// 如果重构失败，返回 None
None
}
// 卡尔曼滤波器的预测步骤
pub fn predict(&mut self) {
// 根据状态转移矩阵更新状态向量
self.state = &self.transition * &self.state;
// 更新状态协方差矩阵
self.covariance = &self.transition * &self.covariance * self.transition.transpose() + &self.process_noise;
}
// 卡尔曼滤波器的更新步骤
pub fn update(&mut self, measurement: f64) {
// 将测量值转换为向量
let measurement_vector = DVector::from_vec(vec![measurement]);
// 计算测量残差
let innovation = &measurement_vector - &self.observation * &self.state;
// 计算测量残差的协方差矩阵
let innovation_covariance = &self.observation * &self.covariance * self.observation.transpose() + &self.measurement_noise;
// 计算卡尔曼增益
if let Some(kalman_gain) = self.compute_kalman_gain(&innovation_covariance) {
// 根据卡尔曼增益更新状态向量
self.state = &self.state + &kalman_gain * innovation;
// 生成单位矩阵
let identity = DMatrix::identity(self.covariance.nrows(), self.covariance.ncols());
// 更新状态协方差矩阵
self.covariance = (identity - &kalman_gain * &self.observation) * &self.covariance;
}
}
// 计算卡尔曼增益
fn compute_kalman_gain(&self, innovation_covariance: &DMatrix<f64>) -> Option<DMatrix<f64>> {
// 尝试直接求逆计算卡尔曼增益
if let Some(inv) = innovation_covariance.clone().try_inverse() {
Some(&self.covariance * self.observation.transpose() * inv)
} else {
// 如果直接求逆失败，使用奇异值分解计算伪逆
let svd = innovation_covariance.clone().svd(true, true);
svd.pseudo_inverse(1e-12)
.ok()
.map(|inv| &self.covariance * self.observation.transpose() * inv)
}
}
// Haar 小波分解
fn haar_decompose(&self, signal: &[f64]) -> (Vec<f64>, Vec<f64>) {
// 获取信号长度
let n = signal.len();
// 如果信号长度小于 2，直接返回原信号和全 0 的细节系数
if n < 2 {
return (signal.to_vec(), vec![0.0; n]);
}
// 初始化近似系数缓冲区
let mut approx = Vec::with_capacity(n / 2);
// 初始化细节系数缓冲区
let mut detail = Vec::with_capacity(n / 2);
// 对信号进行两两分组，计算近似系数和细节系数
for i in (0..n).step_by(2) {
// 左边界（未使用）
let _left_boundary = if i == 0 { i + 1 } else { i - 1 };
// 右边界
let right_boundary = if i + 1 >= n { n - 2 } else { i + 1 };
// 获取当前信号点
let s0 = if i < n { signal[i] } else { signal[n - (i - n) % (n - 1) - 1] };
// 获取下一个信号点
let s1 = if i + 1 < n { signal[i + 1] } else { signal[right_boundary] };
// 计算近似系数
approx.push((s0 + s1) / 2.0);
// 计算细节系数
detail.push((s0 - s1) / 2.0);
}
// 返回近似系数和细节系数
(approx, detail)
}
// Daubechies4 小波分解
fn daubechies4_decompose(&self, signal: &[f64]) -> (Vec<f64>, Vec<f64>) {
// Daubechies4 小波分解的低通滤波器系数
const H0: f64 = 0.4829629131445341;
const H1: f64 = 0.8365163037378079;
const H2: f64 = 0.2241438680420134;
const H3: f64 = -0.1294095225512604;
// 获取信号长度
let n = signal.len();
// 如果信号长度小于 4，直接返回原信号和全 0 的细节系数
if n < 4 {
return (signal.to_vec(), vec![0.0; n]);
}
// 初始化近似系数缓冲区
let mut approx = Vec::with_capacity(n / 2);
// 初始化细节系数缓冲区
let mut detail = Vec::with_capacity(n / 2);
// 定义对称延拓函数，用于处理边界情况
let symmetric_extend = |idx: isize, signal: &[f64]| {
let n = signal.len();
if idx < 0 {
let symmetric_idx = (-idx - 1) as usize;
if symmetric_idx < n { signal[symmetric_idx] } else { *signal.last().unwrap() }
} else if idx >= n as isize {
let symmetric_idx = 2 * n as isize - 2 - idx;
if symmetric_idx < 0 {
signal[0]
} else {
let idx_usize = symmetric_idx as usize;
if idx_usize < n { signal[idx_usize] } else { *signal.last().unwrap() }
}
} else {
signal[idx as usize]
}
};
// 对信号进行两两分组，计算近似系数和细节系数
for i in (0..n).step_by(2) {
let idx_minus2 = i as isize - 2;
let idx_minus1 = i as isize - 1;
let idx_0 = i as isize;
let idx_plus1 = i as isize + 1;
let s_minus2 = symmetric_extend(idx_minus2, signal);
let s_minus1 = symmetric_extend(idx_minus1, signal);
let s0 = symmetric_extend(idx_0, signal);
let s1 = symmetric_extend(idx_plus1, signal);
// 计算近似系数
let a = s_minus2 * H0 +
s_minus1 * H1 +
s0 * H2 +
s1 * H3;
// 计算细节系数
let d = s_minus2 * (-H3) +
s_minus1 * H2 +
s0 * (-H1) +
s1 * H0;
approx.push(a);
detail.push(d);
}
// 返回近似系数和细节系数
(approx, detail)
}
// Haar 小波重构
fn haar_reconstruct(&self, approx: &[f64], detail: &[f64]) -> Vec<f64> {
// 计算重构后信号的长度
let n = approx.len().max(detail.len()) * 2;
// 初始化重构后的信号
let mut signal = vec![0.0; n];
// 定义对称延拓函数，用于处理边界情况
let extend = |arr: &[f64], idx: isize| {
if idx < 0 {
let symmetric_idx = (-idx - 1) as usize;
if symmetric_idx < arr.len() { arr[symmetric_idx] } else { *arr.last().unwrap() }
} else if idx >= arr.len() as isize {
let symmetric_idx = 2 * arr.len() as isize - 2 - idx;
if symmetric_idx < 0 {
arr[0]
} else {
let idx_usize = symmetric_idx as usize;
if idx_usize < arr.len() { arr[idx_usize] } else { *arr.last().unwrap() }
}
} else {
arr[idx as usize]
}
};
// 根据近似系数和细节系数重构信号
for i in 0..n {
if i % 2 == 0 {
let a = extend(approx, (i / 2) as isize);
let d = extend(detail, (i / 2) as isize);
signal[i] = a + d;
} else {
let a = extend(approx, (i / 2) as isize);
let d = extend(detail, (i / 2) as isize);
signal[i] = a - d;
}
}
// 返回重构后的信号
signal
}
// Daubechies4 小波重构
fn daubechies4_reconstruct(&self, approx: &[f64], detail: &[f64]) -> Vec<f64> {
// Daubechies4 小波重构的低通滤波器系数
const G0: f64 = -0.1294095225512604;
const G1: f64 = 0.2241438680420134;
const G2: f64 = 0.8365163037378079;
const G3: f64 = 0.4829629131445341;
// 计算重构后信号的长度
let n = approx.len().max(detail.len()) * 2;
// 初始化重构后的信号
let mut signal = vec![0.0; n];
// 定义对称延拓函数，用于处理边界情况
let extend = |arr: &[f64], idx: isize| {
if idx < 0 {
let symmetric_idx = (-idx - 1) as usize;
if symmetric_idx < arr.len() { arr[symmetric_idx] } else { *arr.last().unwrap() }
} else if idx >= arr.len() as isize {
let symmetric_idx = 2 * arr.len() as isize - 2 - idx;
if symmetric_idx < 0 {
arr[0]
} else {
let idx_usize = symmetric_idx as usize;
if idx_usize < arr.len() { arr[idx_usize] } else { *arr.last().unwrap() }
}
} else {
arr[idx as usize]
}
};
// 根据近似系数和细节系数重构信号
for i in 0..n {
let half_i = i as f64 / 2.0;
let base_idx = half_i.floor() as isize;
let a_idx = base_idx - 1;
let a_idx1 = base_idx;
let a0 = extend(approx, a_idx);
let d0 = extend(detail, a_idx);
let a1 = extend(approx, a_idx1);
let d1 = extend(detail, a_idx1);
let value =
a0 * G2 +
d0 * G3 +
a1 * G0 +
d1 * G1;
signal[i] = value;
}
// 返回重构后的信号
signal
}
// 对小波系数进行阈值处理
fn threshold_coefficients(&self, mut coefficients: Vec<f64>) -> Vec<f64> {
// 对每个系数进行阈值处理
coefficients.iter_mut().for_each(|c| {
if *c > self.threshold {
*c -= self.threshold;
} else if *c < -self.threshold {
*c += self.threshold;
} else {
*c = 0.0;
}
});
// 返回处理后的系数
coefficients
}
// 获取当前状态向量
pub fn get_state(&self) -> &DVector<f64> {
&self.state
}
// 获取当前状态协方差矩阵
pub fn get_covariance(&self) -> &DMatrix<f64> {
&self.covariance
}
}
// 测试模块
#[cfg(test)]
mod tests {
use super::*;
use approx::assert_relative_eq;
use rand::Rng;
// 生成带噪声的正弦信号
fn generate_sine_with_noise(length: usize, noise_level: f64) -> Vec<f64> {
let mut rng = rand::thread_rng();
(0..length)
.map(|i| {
let t = i as f64 * 0.1;
t.sin() + noise_level * (rng.gen_range(-0.5..0.5))
})
.collect()
}
// 测试对称边界处理
#[test]
fn test_symmetric_boundary() {
let signal = vec![1.0, 2.0, 3.0, 4.0];
let mut filter = RealTimeWaveletKalmanFilter::new(
DVector::from_vec(vec![0.0]),
DMatrix::from_diagonal(&DVector::from_vec(vec![1.0])),
DMatrix::from_row_slice(1, 1, &[1.0]),
DMatrix::from_row_slice(1, 1, &[1.0]),
DMatrix::from_diagonal(&DVector::from_vec(vec![0.001])),
DMatrix::from_element(1, 1, 0.1),
WaveletType::Daubechies4,
2,
0.2,
);
// 验证 Daubechies4 分解后的近似系数长度
assert_eq!(filter.daubechies4_decompose(&signal).0.len(), 2);
let short_signal = vec![1.0];
// 验证 Haar 分解后的近似系数长度
assert_eq!(filter.haar_decompose(&short_signal).0.len(), 1);
let decomposed = filter.daubechies4_decompose(&signal);
// 验证分解后的近似系数长度大于 0
assert!(decomposed.0.len() > 0);
}
// 测试带边界的实时信号处理性能
#[test]
fn test_realtime_boundary_performance() {
let mut filter = RealTimeWaveletKalmanFilter::new(
DVector::from_vec(vec![0.0]),
DMatrix::from_diagonal(&DVector::from_vec(vec![1.0])),
DMatrix::from_row_slice(1, 1, &[1.0]),
DMatrix::from_row_slice(1, 1, &[1.0]),
DMatrix::from_diagonal(&DVector::from_vec(vec![0.001])),
DMatrix::from_element(1, 1, 0.1),
WaveletType::Daubechies4,
3,
0.2,
);
// 生成测试数据
let measurements: Vec<f64> = (0..500)
.map(|i| {
if i < 100 {
1.0
} else if i < 300 {
5.0
} else {
2.0
}
})
.collect();
let mut count = 0;
let start = std::time::Instant::now();
// 处理测试数据
for m in &measurements {
if filter.add_data_point(*m).is_some() {
count += 1;
}
}
let duration = start.elapsed();
println!("带边界的信号处理耗时: {:?} ({}点/毫秒)",
duration,
measurements.len() as f64 / duration.as_millis() as f64);
// 验证处理后产生的估计值数量
assert!(count > 450, "应产生大部分估计值");
// 验证处理时间是否符合要求
assert!(duration.as_millis() < 20, "处理过慢");
}
// 测试带边界的实时信号处理
#[test]
fn test_realtime_processing_with_boundary() {
let initial_state = DVector::from_vec(vec![0.0]);
let initial_covariance = DMatrix::from_diagonal(&DVector::from_vec(vec![1.0]));
let transition = DMatrix::from_row_slice(1, 1, &[0.95]);
let observation = DMatrix::from_row_slice(1, 1, &[1.0]);
let process_noise = DMatrix::from_diagonal(&DVector::from_vec(vec![0.001]));
let measurement_noise = DMatrix::from_element(1, 1, 0.1);
let mut filter = RealTimeWaveletKalmanFilter::new(
initial_state,
initial_covariance,
transition,
observation,
process_noise,
measurement_noise,
WaveletType::Daubechies4,
3,
0.3,
);
let n = 128;
// 生成带噪声的正弦信号
let mut measurements = generate_sine_with_noise(n, 0.5);
// 在边界点添加干扰
for i in 0..n {
if i == 0 || i == n - 1 {
measurements[i] += 1.0;
}
}
// 验证前几个数据点是否无法处理
assert!(filter.add_data_point(measurements[0]).is_none());
assert!(filter.add_data_point(measurements[1]).is_none());
assert!(filter.add_data_point(measurements[2]).is_none());
let mut estimates = Vec::with_capacity(n);
// 处理数据并记录估计值
for (i, &m) in measurements.iter().enumerate() {
if let Some(est) = filter.add_data_point(m) {
estimates.push(est);
if i > n - 5 {
println!("Boundary estimate at {}: {:.4} (measured {:.4})", i, est, m);
}
}
}
// 验证处理后产生的估计值数量
assert!(estimates.len() > n - 10, "应产生几乎与输入相同数量的估计值");
// 生成原始正弦信号
let original_signal: Vec<f64> = (0..n).map(|i| (i as f64 * 0.1).sin()).collect();
let mut mse = 0.0;
// 计算均方误差
for (est, orig) in estimates.iter().zip(&original_signal[original_signal.len() - estimates.len()..]) {
mse += (est - orig).powi(2);
}
mse /= estimates.len() as f64;
println!("带边界延拓的实时处理MSE: {:.6}", mse);
// 验证均方误差是否符合要求
assert!(mse < 0.1, "去噪后MSE应较低");
// 计算开始边界失真
let boundary_distortion_start = (estimates[0] - original_signal[original_signal.len() - estimates.len()]).abs();
// 计算结束边界失真
let boundary_distortion_end = (estimates[estimates.len()-1] - *original_signal.last().unwrap()).abs();
println!("开始边界失真: {:.4}, 结束边界失真: {:.4}", boundary_distortion_start, boundary_distortion_end);
// 验证开始边界失真是否符合要求
assert!(boundary_distortion_start < 0.5, "开始边界失真过大");
// 验证结束边界失真是否符合要求
assert!(boundary_distortion_end < 0.5, "结束边界失真过大");
}
// 测试实时 Haar 小波处理
#[test]
fn test_realtime_haar_processing() {
let initial_state = DVector::from_vec(vec![0.0]);
let initial_covariance = DMatrix::from_diagonal(&DVector::from_vec(vec![1.0]));
let transition = DMatrix::from_row_slice(1, 1, &[0.98]);
let observation = DMatrix::from_row_slice(1, 1, &[1.0]);
let process_noise = DMatrix::from_diagonal(&DVector::from_vec(vec![0.001]));
let measurement_noise = DMatrix::from_element(1, 1, 0.15);
let mut filter = RealTimeWaveletKalmanFilter::new(
initial_state,
initial_covariance,
transition,
observation,
process_noise,
measurement_noise,
WaveletType::Haar,
4,
0.4,
);
let mut measurements = vec![];
// 生成测试数据
for i in 0..100 {
if i < 30 {
measurements.push(1.0 + rand::thread_rng().gen_range(-0.5..0.5));
} else if i < 70 {
measurements.push(-1.0 + rand::thread_rng().gen_range(-0.5..0.5));
} else {
measurements.push(2.0 + rand::thread_rng().gen_range(-0.5..0.5));
}
}
// 在边界点添加干扰
measurements[0] += 1.0;
measurements[99] += 1.0;
let mut estimates = Vec::with_capacity(measurements.len());
// 处理数据并记录估计值
for (i, &m) in measurements.iter().enumerate() {
if let Some(est) = filter.add_data_point(m) {
estimates.push(est);
if i == 0 || i == 99 {
println!("边界点 {}: 测量值={:.4}, 估计值={:.4}", i, m, est);
}
}
}
// 获取第一个阶跃点的估计值
let step1 = estimates[35].abs();
// 获取第二个阶跃点的估计值
let step2 = estimates[75].abs();
// 验证是否检测到第一个阶跃
assert!(step1 > 0.8, "未检测到第一个阶跃");
// 验证是否检测到第二个阶跃
assert!(step2 > 1.5, "未检测到第二个阶跃");
// 验证阶跃过渡是否清晰
assert!(step2 - step1 > 1.0, "阶跃过渡不清晰");
// 计算开始边界失真
let start_distortion = (estimates[0] - 2.0).abs();
// 计算结束边界失真
let end_distortion = (estimates[99] - 3.0).abs();
println!("开始边界失真: {:.4}, 结束边界失真: {:.4}", start_distortion, end_distortion);
// 验证开始边界失真是否符合要求
assert!(start_distortion < 1.0, "开始边界失真过大");
// 验证结束边界失真是否符合要求
assert!(end_distortion < 1.0, "结束边界失真过大");
}
}
// 主函数，演示实时小波卡尔曼滤波器的使用
fn main() {
// 初始化状态向量
let initial_state = DVector::from_vec(vec![0.0]);
// 初始化状态协方差矩阵
let initial_covariance = DMatrix::from_diagonal(&DVector::from_vec(vec![1.0]));
// 初始化状态转移矩阵
let transition = DMatrix::from_row_slice(1, 1, &[0.95]);
// 初始化观测矩阵
let observation = DMatrix::from_row_slice(1, 1, &[1.0]);
// 初始化过程噪声协方差矩阵
let process_noise = DMatrix::from_diagonal(&DVector::from_vec(vec![0.001]));
// 初始化测量噪声协方差矩阵
let measurement_noise = DMatrix::from_element(1, 1, 0.1);
// 创建实时小波卡尔曼滤波器实例
let mut filter = RealTimeWaveletKalmanFilter::new(
initial_state,
initial_covariance,
transition,
observation,
process_noise,
measurement_noise,
WaveletType::Daubechies4,
3,
0.2,
);
// 定义测试数据
let measurements = vec![1.0, 1.1, 0.9, 1.2, 0.8, 1.3, 0.7];
// 处理测试数据并输出去噪后的估计值
for m in measurements {
if let Some(estimate) = filter.add_data_point(m) {
println!("去噪后的估计值: {}", estimate);
}
}
}
扫一扫
739
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
