使用Rust原生实现小波卡尔曼滤波算法

一、算法原理概述

1. 小波变换（Wavelet Transform）

   • 通过多尺度分解将信号分为高频（细节）和低频（近似）部分，高频通常包含噪声，低频保留主体信息。
   • 使用Haar小波（计算高效）进行快速分解与重构：

     // Haar小波分解公式
     approx = (x[2i] + x[2i+1]) / 2.0;
     detail = (x[2i] - x[2i+1]) / 2.0;
     

2. 卡尔曼滤波（Kalman Filter）

   • 预测-更新两阶段递归估计[citation:5][citation:6]：
     • 预测：基于状态方程预估当前状态和误差协方差

       Xpred=A⋅Xprev+B⋅uPpred=A⋅Pprev⋅AT+QXpred​=A⋅Xprev​+B⋅uPpred​=A⋅Pprev​⋅AT+Q

     • 更新：结合观测值修正预测值，计算卡尔曼增益K

       K=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​

3. 小波卡尔曼融合

   • 先对原始信号小波分解，对不同频段独立应用卡尔曼滤波，最后重构信号
   • 高频部分使用更高测量噪声协方差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_prevself.cov = self.a * self.cov * self.a + self.q; // P_pred = A*P*A^T + Qinfo!("执行单步预测后，状态: {}, 协方差: {}", 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); // 卡尔曼增益Kself.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_predinfo!("执行单步更新后，状态: {}, 协方差: {}", 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!("程序结束");
}