Wiener滤波算法的C++实现

2019独角兽企业重金招聘Python工程师标准>>>

头文件：

/*
 * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), zmjerry@163.com
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 2 or any later version.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details. A copy of the GNU General Public License is available at:
 * http://www.fsf.org/licensing/licenses
 */


/*****************************************************************************
 *                                  levinson.h
 *
 * If the coefficient matrix of a linear equations is Toeplitz, then it can
 * be solved in a high computational efficiency way through Levinson-Durbin
 * algorithm. The subroutiones in this file will be used for solving Wiener
 * -Hopf equeations in Wiener filtring and Yule- Walker equations in
 * parametric spectrum estimation, respectively.
 *
 * Zhang Ming, 2010-11, Xi'an Jiaotong University.
 *****************************************************************************/


#ifndef LEVINSON_H
#define LEVINSON_H


#include <vector.h>


namespace splab
{

    template<typename Type>
    Vector<Type> levinson( const Vector<Type>&, const Vector<Type>& );

    template<typename Type>
    Vector<Type> levinson( const Vector<Type>&, Type& );


    #include <levinson-impl.h>

}
// namespace splab


#endif
// LEVINSON_H

/*
 * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), zmjerry@163.com
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 2 or any later version.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details. A copy of the GNU General Public License is available at:
 * http://www.fsf.org/licensing/licenses
 */


/*****************************************************************************
 *                                   wiener.h
 *
 * Wiener Filter.
 *
 * The goal of the Wiener filter is to filter out noise that has corrupted
 * a signal. It is based on a statistical approach.
 *
 * Wiener filters are characterized by the following:
 * Assumption:  signal and (additive) noise are stationary linear stochastic
 *              processes with known spectral characteristics or known
 *              autocorrelation and cross-correlation.
 * Requirement: the filter must be physically realizable/causal (this
 *              requirement can be dropped, resulting in a non-causal solution).
 * Performance: criterion: minimum mean-square error (MMSE).
 *
 * And The correlation matrix is a symmetric Toeplitz matrix, so we can use the
 * efficient algorithm, Levinson-Durbin algorithm, to solve the Wiener-Hopf
 * Equations.
 *
 * Zhang Ming, 2010-10, Xi'an Jiaotong University.
 *****************************************************************************/


#ifndef WIENER_H
#define WIENER_H


#include <vector.h>
#include <correlation.h>
#include <levinson.h>


namespace splab
{

    template<typename Type>
    Vector<Type> wienerFilter( const Vector<Type>&,
                               const Vector<Type>&, int );

    template<typename Type>
    Vector<Type> wienerPredictor( const Vector<Type>&, int );


    #include <wiener-impl.h>

}
// namespace splab


#endif
// WIENER_H

实现文件：

/*
 * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), zmjerry@163.com
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 2 or any later version.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details. A copy of the GNU General Public License is available at:
 * http://www.fsf.org/licensing/licenses
 */


/*****************************************************************************
 *                               levinson-impl.h
 *
 * Implementationfor Levinson-Durbin alogrithm.
 *
 * Zhang Ming, 2010-11, Xi'an Jiaotong University.
 *****************************************************************************/


/**
 * Levinson algorithm for solving Toeplitz equations.
 * t    : t(0), t(1), ..., t(n-1) of Toeplitz coefficient matrix
 * b    : constant vector
 */
template <typename Type>
Vector<Type> levinson( const Vector<Type> &t, const Vector<Type> &b )
{
    assert( t.size() == b.size() );

    int n = t.size();
	Type alpha, beta, q, c, omega;
	Vector<Type> y(n), yy(n), x(n);

	alpha = t[0];
	if( abs(alpha) < EPS )
	{
		cerr << "The matrix is ill-conditioned!" << endl;
		return x;
	}
	y[0] = 1;
	x[0] = b[0] / alpha;

	for( int k=1; k<n; ++k )
	{
		q = 0;
		beta = 0;
		for( int j=0; j<k; ++j )
		{
			q += x[j] * t[k-j];
			beta += y[j] * t[j+1];
		}
		c = -beta / alpha;

		yy[0] = c * y[k-1];
		y[k] = y[k-1];
		for( int i=1; i<k; ++i )
			yy[i] = y[i-1] + c*y[k-i-1];
		yy[k] = y[k-1];

		alpha += c*beta;
		if( abs(alpha) < EPS )
		{
			cerr << "The matrix is ill-conditioned!" << endl;
			return x;
		}

		omega = (b[k]-q) / alpha;
		for( int i=0; i<k; ++i )
		{
			x[i] += omega*yy[i];
			y[i] = yy[i];
		}
		x[k] = omega*y[k];
	}

	return x;
}


/**
 * Levinson-Durbin algorithm for solving Youle-Walker equations.
 * rn       : r(0), r(1), ..., r(p)
 * sigma2   : the variance of exciting white noise
 */
template <typename Type>
Vector<Type> levinson( const Vector<Type> &rn, Type &sigma2 )
{
    int p = rn.size()-1;
    Type tmp;
    Vector<Type> ak(p+1), akPrev(p+1);

    ak[0] = Type(1.0);
    sigma2 = rn[0];
    ak[1] = -rn[1]/sigma2;
    sigma2 *= 1 - ak[1]*ak[1];

    for( int k=2; k<=p; ++k )
    {
        tmp = 0;
        for( int i=0; i<k; ++i )
            tmp += ak[i]*rn[k-i];
        ak[k] = -tmp/sigma2;

        for( int i=1; i<k; ++i )
            akPrev[i] = ak[i] + ak[k]*ak[k-i];
        for( int i=1; i<k; ++i )
            ak[i] = akPrev[i];

        sigma2 *= 1 - ak[k]*ak[k];
    }

	return ak;
}

/*
 * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), zmjerry@163.com
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 2 or any later version.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details. A copy of the GNU General Public License is available at:
 * http://www.fsf.org/licensing/licenses
 */


/*****************************************************************************
 *                              wiener-impl.h
 *
 * Implementation for Wiener Filter.
 *
 * Zhang Ming, 2010-10, Xi'an Jiaotong University.
 *****************************************************************************/


/**
 * By given the observed signal "xn" and desired signal "dn", this routine
 * return the coefficients for Wiener filter with "p" order.
 */
template <typename Type>
Vector<Type> wienerFilter( const Vector<Type> &xn,
                           const Vector<Type> &dn, int p )
{
    int N = xn.size();
    assert( dn.size() == N );

    // auto-correlation and corss-correlation
    Vector<Type> Rxx = fastCorr( xn, "unbiased" );
    Vector<Type> Rdx = fastCorr( dn, xn, "unbiased" );

    Vector<Type> tn(p+1), bn(p+1);
    for( int i=0; i<=p; ++i )
    {
        tn[i] = Rxx[N-1+i];
        bn[i] = Rdx[N-1+i];
    }

    // solving Wiener-Hopf equations
    return levinson( tn, bn );
}


/**
 * One step Wiener predictor by using a "p" order filter. The input
 * signal "xn" should be much longer the "p" in order to have a more
 * precision estimation of the Correlation Matrix of "xn".
 */
template <typename Type>
Vector<Type> wienerPredictor( const Vector<Type> &xn, int p )
{
    int N = xn.size();

    // auto-correlation and corss-correlation
    Vector<Type> Rxx = fastCorr( xn, "unbiased" );
    Vector<Type> tn(p+1), bn(p+1), predictor(p);

    for( int i=0; i<=p; ++i )
        tn[i] = Rxx[N-1+i];
    bn(1) = Type(1.0);

    // solving Yule-Walker equations
    Vector<Type> tmp = levinson( tn, bn );

    for( int i=1; i<=p; ++i )
        predictor(i) = -tmp(i+1) / tmp(1);

    return predictor;
}

测试代码：

/*****************************************************************************
 *                                wiener_test.h
 *
 * Wiener filter testing.
 *
 * Zhang Ming, 2010-10, Xi'an Jiaotong University.
 *****************************************************************************/


#define BOUNDS_CHECK

#include <iostream>
#include <iomanip>
#include <wiener.h>
#include <random.h>
#include <vectormath.h>


using namespace std;
using namespace splab;


typedef double  Type;
const   int     N = 1024;
const   int     M = 12;
const   int     fOrder = 8;
const   int     pOrder = 3;


int main()
{
    Vector<Type> tn(N), dn(N), vn(N), xn(N), yn(N);
    tn = linspace( Type(0.0), Type(2*TWOPI), N );
    dn = sin(tn);
    vn = randn( 37, Type(0.0), Type(1.0), N );
    xn = dn + vn;

    Vector<Type> hn = wienerFilter( xn, dn, fOrder );
    cout << "Wiener filter hn:   " << hn << endl;
    yn = wkeep( conv(xn,hn), N, "left" );
    cout << "original relative error:   " << norm(dn-xn)/norm(dn) << endl;
    cout << "filtered relative error:   " << norm(dn-yn)/norm(dn) << endl << endl;

    Vector<Type> sn(M);
    for( int i=0; i<M; ++i )
        sn[i] = sin( Type(i*TWOPI/10) );
    Vector<Type> pn = wienerPredictor( sn, pOrder );
    Vector<Type> snPred = wkeep( conv(sn,pn), M, "left" );
    cout << "Wiener predictor pn:   " << pn << endl;
    cout << "original\tpredicted\terror" << endl;
    Type realValue = sin( Type(M*TWOPI/10) );
    cout << setiosflags(ios::fixed) << setprecision(4)
         << realValue << "\t\t" << snPred[M-1] << "\t\t"
         << abs(realValue-snPred[M-1]) << endl << endl;

    return 0;
}

运行结果：

Wiener filter hn:   size: 9 by 1
0.0903079
0.0858067
0.0813221
0.0870775
0.0968708
0.0888659
0.0844214
0.0901495
0.0965093

original relative error:   1.43689
filtered relative error:   0.416294

Wiener predictor pn:   size: 3 by 1
0.606355
0.699992
-1.03413

original        predicted       error
0.9511          0.9643          0.0132


Process returned 0 (0x0)   execution time : 0.094 s
Press any key to continue.

转载于:https://my.oschina.net/zmjerry/blog/8515