使用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_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!("程序结束");
}