Hi All,
I have implemented wiener filter for noise cancellation on C6713 DSK
board. My results don't match with matlab implementation. I believe
this may be due to word alignment issues with my code. As TI FFT
routines expect data to be double-word aligned, can you please check
my code whether it is calling DSP Libraries in a correct fashion.
Thanks in advance,
Gaurav
#include
#include
#include
#include
#include "DSPF_sp_cfftr2_dit.h"
#include "DSPF_sp_bitrev_cplx.h"
#include "DSPF_sp_icfftr2_dif.h"
#include "original_clean.h" // clean signal vector
#include "noisy.h" // noisy signal
#include "freq_vector.h" // frequency axis
//#include "window.h"
//#include "twiddle.h"
#include "bit_reverse_table.h" // bit reverse table, which is
commented below
// short table[32] = {0, 16, 8, 24, 4, 20, 12, 28, 2, 18, 10, 26, 6,
22, 14, 30, 1, 17, 9, 25, 5, 21, 13, 29, 3, 19, 11, 27, 7, 23, 15,
31};
#define N 512
#define HALFWINDOW_SIZE 5
#define MAX(a, b) (a > b ? a : b)
void compute_psd(float *data, float *x);
float win_data[N];
#pragma DATA_ALIGN(win_data, 64);
float data[4096];
float e[4096];
// output vector containing frequency samples
float x[2 * N];
#pragma DATA_ALIGN(x, 64);
// twiddle factor
float w[512]={ 1.00000000, 0.00000000, 0.99992470,
0.01227154, 0.99969882, 0.02454123, 0.99932238, 0.03680722,
0.99879546, 0.04906767, 0.99811811, 0.06132074, 0.99729046,
0.07356456, 0.99631261, 0.08579731, 0.99518473, 0.09801714,
0.99390697, 0.11022221, 0.99247953, 0.12241068, 0.99090264,
0.13458071, 0.98917651, 0.14673047, 0.98730142, 0.15885814,
0.98527764, 0.17096189, 0.98310549, 0.18303989, 0.98078528,
0.19509032, 0.97831737, 0.20711138, 0.97570213, 0.21910124,
0.97293995, 0.23105811, 0.97003125, 0.24298018, 0.96697647,
0.25486566, 0.96377607, 0.26671276, 0.96043052, 0.27851969,
0.95694034, 0.29028468, 0.95330604, 0.30200595, 0.94952818,
0.31368174, 0.94560733, 0.32531029, 0.94154407, 0.33688985,
0.93733901, 0.34841868, 0.93299280, 0.35989504, 0.92850608,
0.37131719, 0.92387953, 0.38268343, 0.91911385, 0.39399204,
0.91420976, 0.40524131, 0.90916798, 0.41642956, 0.90398929,
0.42755509, 0.89867447, 0.43861624, 0.89322430, 0.44961133,
0.88763962, 0.46053871, 0.88192126, 0.47139674, 0.87607009,
0.48218377, 0.87008699, 0.49289819, 0.86397286, 0.50353838,
0.85772861, 0.51410274, 0.85135519, 0.52458968, 0.84485357,
0.53499762, 0.83822471, 0.54532499, 0.83146961, 0.55557023,
0.82458930, 0.56573181, 0.81758481, 0.57580819, 0.81045720,
0.58579786, 0.80320753, 0.59569930, 0.79583690, 0.60551104,
0.78834643, 0.61523159, 0.78073723, 0.62485949, 0.77301045,
0.63439328, 0.76516727, 0.64383154, 0.75720885, 0.65317284,
0.74913639, 0.66241578, 0.74095113, 0.67155895, 0.73265427,
0.68060100, 0.72424708, 0.68954054, 0.71573083, 0.69837625,
0.70710678, 0.70710678, 0.69837625, 0.71573083, 0.68954054,
0.72424708, 0.68060100, 0.73265427, 0.67155895, 0.74095113,
0.66241578, 0.74913639, 0.65317284, 0.75720885, 0.64383154,
0.76516727, 0.63439328, 0.77301045, 0.62485949, 0.78073723,
0.61523159, 0.78834643, 0.60551104, 0.79583690, 0.59569930,
0.80320753, 0.58579786, 0.81045720, 0.57580819, 0.81758481,
0.56573181, 0.82458930, 0.55557023, 0.83146961, 0.54532499,
0.83822471, 0.53499762, 0.84485357, 0.52458968, 0.85135519,
0.51410274, 0.85772861, 0.50353838, 0.86397286, 0.49289819,
0.87008699, 0.48218377, 0.87607009, 0.47139674, 0.88192126,
0.46053871, 0.88763962, 0.44961133, 0.89322430, 0.43861624,
0.89867447, 0.42755509, 0.90398929, 0.41642956, 0.90916798,
0.40524131, 0.91420976, 0.39399204, 0.91911385, 0.38268343,
0.92387953, 0.37131719, 0.92850608, 0.35989504, 0.93299280,
0.34841868, 0.93733901, 0.33688985, 0.94154407, 0.32531029,
0.94560733, 0.31368174, 0.94952818, 0.30200595, 0.95330604,
0.29028468, 0.95694034, 0.27851969, 0.96043052, 0.26671276,
0.96377607, 0.25486566, 0.96697647, 0.24298018, 0.97003125,
0.23105811, 0.97293995, 0.21910124, 0.97570213, 0.20711138,
0.97831737, 0.19509032, 0.98078528, 0.18303989, 0.98310549,
0.17096189, 0.98527764, 0.15885814, 0.98730142, 0.14673047,
0.98917651, 0.13458071, 0.99090264, 0.12241068, 0.99247953,
0.11022221, 0.99390697, 0.09801714, 0.99518473, 0.08579731,
0.99631261, 0.07356456, 0.99729046, 0.06132074, 0.99811811,
0.04906767, 0.99879546, 0.03680722, 0.99932238, 0.02454123,
0.99969882, 0.01227154, 0.99992470, 0.00000000, 1.00000000, -
0.01227154, 0.99992470, -0.02454123, 0.99969882, -0.03680722,
0.99932238, -0.04906767, 0.99879546, -0.06132074, 0.99811811, -
0.07356456, 0.99729046, -0.08579731, 0.99631261, -0.09801714,
0.99518473, -0.11022221, 0.99390697, -0.12241068, 0.99247953, -
0.13458071, 0.99090264, -0.14673047, 0.98917651, -0.15885814,
0.98730142, -0.17096189, 0.98527764, -0.18303989, 0.98310549, -
0.19509032, 0.98078528, -0.20711138, 0.97831737, -0.21910124,
0.97570213, -0.23105811, 0.97293995, -0.24298018, 0.97003125, -
0.25486566, 0.96697647, -0.26671276, 0.96377607, -0.27851969,
0.96043052, -0.29028468, 0.95694034, -0.30200595, 0.95330604, -
0.31368174, 0.94952818, -0.32531029, 0.94560733, -0.33688985,
0.94154407, -0.34841868, 0.93733901, -0.35989504, 0.93299280, -
0.37131719, 0.92850608, -0.38268343, 0.92387953, -0.39399204,
0.91911385, -0.40524131, 0.91420976, -0.41642956, 0.90916798, -
0.42755509, 0.90398929, -0.43861624, 0.89867447, -0.44961133,
0.89322430, -0.46053871, 0.88763962, -0.47139674, 0.88192126, -
0.48218377, 0.87607009, -0.49289819, 0.87008699, -0.50353838,
0.86397286, -0.51410274, 0.85772861, -0.52458968, 0.85135519, -
0.53499762, 0.84485357, -0.54532499, 0.83822471, -0.55557023,
0.83146961, -0.56573181, 0.82458930, -0.57580819, 0.81758481, -
0.58579786, 0.81045720, -0.59569930, 0.80320753, -0.60551104,
0.79583690, -0.61523159, 0.78834643, -0.62485949, 0.78073723, -
0.63439328, 0.77301045, -0.64383154, 0.76516727, -0.65317284,
0.75720885, -0.66241578, 0.74913639, -0.67155895, 0.74095113, -
0.68060100, 0.73265427, -0.68954054, 0.72424708, -0.69837625,
0.71573083, -0.70710678, 0.70710678, -0.71573083, 0.69837625, -
0.72424708, 0.68954054, -0.73265427, 0.68060100, -0.74095113,
0.67155895, -0.74913639, 0.66241578, -0.75720885, 0.65317284, -
0.76516727, 0.64383154, -0.77301045, 0.63439328, -0.78073723,
0.62485949, -0.78834643, 0.61523159, -0.79583690, 0.60551104, -
0.80320753, 0.59569930, -0.81045720, 0.58579786, -0.81758481,
0.57580819, -0.82458930, 0.56573181, -0.83146961, 0.55557023, -
0.83822471, 0.54532499, -0.84485357, 0.53499762, -0.85135519,
0.52458968, -0.85772861, 0.51410274, -0.86397286, 0.50353838, -
0.87008699, 0.49289819, -0.87607009, 0.48218377, -0.88192126,
0.47139674, -0.88763962, 0.46053871, -0.89322430, 0.44961133, -
0.89867447, 0.43861624, -0.90398929, 0.42755509, -0.90916798,
0.41642956, -0.91420976, 0.40524131, -0.91911385, 0.39399204, -
0.92387953, 0.38268343, -0.92850608, 0.37131719, -0.93299280,
0.35989504, -0.93733901, 0.34841868, -0.94154407, 0.33688985, -
0.94560733, 0.32531029, -0.94952818, 0.31368174, -0.95330604,
0.30200595, -0.95694034, 0.29028468, -0.96043052, 0.27851969, -
0.96377607, 0.26671276, -0.96697647, 0.25486566, -0.97003125,
0.24298018, -0.97293995, 0.23105811, -0.97570213, 0.21910124, -
0.97831737, 0.20711138, -0.98078528, 0.19509032, -0.98310549,
0.18303989, -0.98527764, 0.17096189, -0.98730142, 0.15885814, -
0.98917651, 0.14673047, -0.99090264, 0.13458071, -0.99247953,
0.12241068, -0.99390697, 0.11022221, -0.99518473, 0.09801714, -
0.99631261, 0.08579731, -0.99729046, 0.07356456, -0.99811811,
0.06132074, -0.99879546, 0.04906767, -0.99932238, 0.03680722, -
0.99969882, 0.02454123, -0.99992470, 0.01227154};
#pragma DATA_ALIGN(w, 64);
// hanning window coeffcients
float win_coeff[512]={ 0.00003750, 0.00015000, 0.00033749,
0.00059993, 0.00093728, 0.00134950, 0.00183652, 0.00239827,
0.00303466, 0.00374561, 0.00453099, 0.00539071, 0.00632461,
0.00733258, 0.00841445, 0.00957006, 0.01079924, 0.01210181,
0.01347756, 0.01492630, 0.01644781, 0.01804185, 0.01970819,
0.02144658, 0.02325676, 0.02513845, 0.02709138, 0.02911525,
0.03120975, 0.03337458, 0.03560941, 0.03791390, 0.04028771,
0.04273047, 0.04524184, 0.04782142, 0.05046883, 0.05318368,
0.05596555, 0.05881404, 0.06172870, 0.06470911, 0.06775482,
0.07086537, 0.07404030, 0.07727912, 0.08058135, 0.08394650,
0.08737407, 0.09086353, 0.09441436, 0.09802604, 0.10169802,
0.10542975, 0.10922066, 0.11307020, 0.11697778, 0.12094282,
0.12496472, 0.12904287, 0.13317668, 0.13736551, 0.14160874,
0.14590574, 0.15025585, 0.15465842, 0.15911281, 0.16361832,
0.16817430, 0.17278006, 0.17743490, 0.18213813, 0.18688904,
0.19168692, 0.19653105, 0.20142070, 0.20635515, 0.21133364,
0.21635544, 0.22141979, 0.22652592, 0.23167308, 0.23686049,
0.24208738, 0.24735295, 0.25265642, 0.25799700, 0.26337388,
0.26878626, 0.27423332, 0.27971425, 0.28522822, 0.29077441,
0.29635199, 0.30196012, 0.30759795, 0.31326465, 0.31895936,
0.32468122, 0.33042939, 0.33620299, 0.34200117, 0.34782304,
0.35366775, 0.35953440, 0.36542213, 0.37133004, 0.37725726,
0.38320288, 0.38916603, 0.39514581, 0.40114131, 0.40715165,
0.41317591, 0.41921320, 0.42526260, 0.43132322, 0.43739414,
0.44347445, 0.44956324, 0.45565960, 0.46176261, 0.46787135,
0.47398491, 0.48010238, 0.48622283, 0.49234535, 0.49846901,
0.50459291, 0.51071611, 0.51683771, 0.52295679, 0.52907241,
0.53518368, 0.54128967, 0.54738947, 0.55348216, 0.55956682,
0.56564255, 0.57170843, 0.57776356, 0.58380702, 0.58983790,
0.59585532, 0.60185835, 0.60784610, 0.61381767, 0.61977218,
0.62570871, 0.63162638, 0.63752432, 0.64340162, 0.64925741,
0.65509080, 0.66090094, 0.66668694, 0.67244793, 0.67818305,
0.68389145, 0.68957226, 0.69522463, 0.70084771, 0.70644067,
0.71200266, 0.71753284, 0.72303040, 0.72849449, 0.73392432,
0.73931904, 0.74467787, 0.75000000, 0.75528462, 0.76053095,
0.76573820, 0.77090558, 0.77603232, 0.78111766, 0.78616083,
0.79116106, 0.79611763, 0.80102977, 0.80589675, 0.81071785,
0.81549233, 0.82021949, 0.82489862, 0.82952900, 0.83410995,
0.83864079, 0.84312082, 0.84754938, 0.85192581, 0.85624944,
0.86051963, 0.86473574, 0.86889714, 0.87300320, 0.87705330,
0.88104685, 0.88498323, 0.88886186, 0.89268216, 0.89644355,
0.90014547, 0.90378737, 0.90736869, 0.91088891, 0.91434748,
0.91774391, 0.92107766, 0.92434825, 0.92755518, 0.93069798,
0.93377617, 0.93678928, 0.93973688, 0.94261850, 0.94543374,
0.94818215, 0.95086333, 0.95347687, 0.95602240, 0.95849951,
0.96090784, 0.96324703, 0.96551673, 0.96771660, 0.96984631,
0.97190554, 0.97389397, 0.97581132, 0.97765729, 0.97943160,
0.98113400, 0.98276422, 0.98432203, 0.98580718, 0.98721945,
0.98855864, 0.98982453, 0.99101695, 0.99213571, 0.99318065,
0.99415161, 0.99504843, 0.99587100, 0.99661918, 0.99729286,
0.99789194, 0.99841634, 0.99886597, 0.99924076, 0.99954066,
0.99976563, 0.99991562, 0.99999062, 0.99999062, 0.99991562,
0.99976563, 0.99954066, 0.99924076, 0.99886597, 0.99841634,
0.99789194, 0.99729286, 0.99661918, 0.99587100, 0.99504843,
0.99415161, 0.99318065, 0.99213571, 0.99101695, 0.98982453,
0.98855864, 0.98721945, 0.98580718, 0.98432203, 0.98276422,
0.98113400, 0.97943160, 0.97765729, 0.97581132, 0.97389397,
0.97190554, 0.96984631, 0.96771660, 0.96551673, 0.96324703,
0.96090784, 0.95849951, 0.95602240, 0.95347687, 0.95086333,
0.94818215, 0.94543374, 0.94261850, 0.93973688, 0.93678928,
0.93377617, 0.93069798, 0.92755518, 0.92434825, 0.92107766,
0.91774391, 0.91434748, 0.91088891, 0.90736869, 0.90378737,
0.90014547, 0.89644355, 0.89268216, 0.88886186, 0.88498323,
0.88104685, 0.87705330, 0.87300320, 0.86889714, 0.86473574,
0.86051963, 0.85624944, 0.85192581, 0.84754938, 0.84312082,
0.83864079, 0.83410995, 0.82952900, 0.82489862, 0.82021949,
0.81549233, 0.81071785, 0.80589675, 0.80102977, 0.79611763,
0.79116106, 0.78616083, 0.78111766, 0.77603232, 0.77090558,
0.76573820, 0.76053095, 0.75528462, 0.75000000, 0.74467787,
0.73931904, 0.73392432, 0.72849449, 0.72303040, 0.71753284,
0.71200266, 0.70644067, 0.70084771, 0.69522463, 0.68957226,
0.68389145, 0.67818305, 0.67244793, 0.66668694, 0.66090094,
0.65509080, 0.64925741, 0.64340162, 0.63752432, 0.63162638,
0.62570871, 0.61977218, 0.61381767, 0.60784610, 0.60185835,
0.59585532, 0.58983790, 0.58380702, 0.57776356, 0.57170843,
0.56564255, 0.55956682, 0.55348216, 0.54738947, 0.54128967,
0.53518368, 0.52907241, 0.52295679, 0.51683771, 0.51071611,
0.50459291, 0.49846901, 0.49234535, 0.48622283, 0.48010238,
0.47398491, 0.46787135, 0.46176261, 0.45565960, 0.44956324,
0.44347445, 0.43739414, 0.43132322, 0.42526260, 0.41921320,
0.41317591, 0.40715165, 0.40114131, 0.39514581, 0.38916603,
0.38320288, 0.37725726, 0.37133004, 0.36542213, 0.35953440,
0.35366775, 0.34782304, 0.34200117, 0.33620299, 0.33042939,
0.32468122, 0.31895936, 0.31326465, 0.30759795, 0.30196012,
0.29635199, 0.29077441, 0.28522822, 0.27971425, 0.27423332,
0.26878626, 0.26337388, 0.25799700, 0.25265642, 0.24735295,
0.24208738, 0.23686049, 0.23167308, 0.22652592, 0.22141979,
0.21635544, 0.21133364, 0.20635515, 0.20142070, 0.19653105,
0.19168692, 0.18688904, 0.18213813, 0.17743490, 0.17278006,
0.16817430, 0.16361832, 0.15911281, 0.15465842, 0.15025585,
0.14590574, 0.14160874, 0.13736551, 0.13317668, 0.12904287,
0.12496472, 0.12094282, 0.11697778, 0.11307020, 0.10922066,
0.10542975, 0.10169802, 0.09802604, 0.09441436, 0.09086353,
0.08737407, 0.08394650, 0.08058135, 0.07727912, 0.07404030,
0.07086537, 0.06775482, 0.06470911, 0.06172870, 0.05881404,
0.05596555, 0.05318368, 0.05046883, 0.04782142, 0.04524184,
0.04273047, 0.04028771, 0.03791390, 0.03560941, 0.03337458,
0.03120975, 0.02911525, 0.02709138, 0.02513845, 0.02325676,
0.02144658, 0.01970819, 0.01804185, 0.01644781, 0.01492630,
0.01347756, 0.01210181, 0.01079924, 0.00957006, 0.00841445,
0.00733258, 0.00632461, 0.00539071, 0.00453099, 0.00374561,
0.00303466, 0.00239827, 0.00183652, 0.00134950, 0.00093728,
0.00059993, 0.00033749, 0.00015000, 0.00003750};
#pragma DATA_ALIGN(win_coeff, 64);
float de[4096 + 2 * HALFWINDOW_SIZE];
float fft_coeff[512];
float fft_coeff_real;
float fft_coeff_img;
float fft_vector[257];
float error_filtered[4096];
float df2[4096];
void main()
{
float temp = 0;
float temp1[12];
float lmean = 0.0;
float lvar = 0.0;
float temp_var = 0.0;
float error_filtered_Square = 0.0;
float SNR_Filtered;
float Clean_d_Square = 0.0;
float e_Square = 0.0;
int index, i, n, j, k, count;
double Test_Data_snr = 0.0;
float varnoise, datapower, signalpower, Estimated_SNR;
float KMU = 0.00065;
datapower = 0.0;
memset(fft_coeff, 0, 512 * sizeof(float));
memset(df2, 0, 4096 * sizeof(float));
memset(de, 0, 5 * sizeof(float));
memcpy(de + 5, d, 4096 * sizeof(float));
memset(de + 4101, 0, 5 * sizeof(float));
for (index = 0; index < 4096; index++)
{
e[index] = Clean_d[index] - d[index];
}
for (index = 0; index < 4096; index++)
{
Clean_d_Square += Clean_d[index] * Clean_d[index];
e_Square += e[index] * e[index];
}
//Test_Data_snr = 10 * log10(Clean_d_Square/e_Square);
memcpy(data, d, 4096 * sizeof(float));
k = 0;
for (n = 0; n < 8; n++)
{
for (index = 0; index < N; index++, k++)
{
// applying hanning window function to the data set
win_data[index] = win_coeff[index] * data[k];
}
// compute Power spectral density
compute_psd(win_data, x);
//i = 0;
for (index = 0; index < N; index++)
{
fft_coeff_real = x[2 * index];
fft_coeff_img = x[2 * index + 1];
fft_coeff[index] += ((fft_coeff_real * fft_coeff_real) +
(fft_coeff_img * fft_coeff_img));
}
}
for ( i = 0; i < N; i++)
fft_coeff[i] = fft_coeff[i] * KMU; // scaling
memcpy(fft_vector, fft_coeff, 257 * sizeof(float)); // copy N/2 +
1 coefficients, fft is symmetric
count = 0;
for ( i = 0; i < 257; i++ )
{
if ( freq_vector[i] >= 0.4 )
{
temp += fft_vector[i];
count++;
}
}
varnoise = temp/count; // estimated noise power
for (i = 0; i < 257; i++)
{
datapower += fft_vector[i];
}
datapower = datapower/257;
signalpower = datapower - varnoise;
//Estimated_SNR = 10 * log10(signalpower/varnoise);
for (j = 0; j < 4096; j++)
{
memcpy(temp1, de + j, 11 * sizeof(float));
for (k = 0; k < 11; k++)
{
lmean += temp1[k];
temp_var += temp1[k] * temp1[k];
}
lmean = lmean/11;
lvar = (temp_var/(2 * HALFWINDOW_SIZE + 1 )) - (lmean * lmean);
df2[j] = lmean + (MAX(0, lvar - varnoise)/MAX(lvar, varnoise))*(de[j
+ 6] - lmean);
}
for (index = 0; index < 4096; index++)
{
error_filtered[index] = Clean_d[index] - df2[index];
}
for (index = 0; index < 4096; index++)
{
error_filtered_Square += error_filtered[index] * error_filtered
[index];
}
// SNR_Filtered = 10 * log10(Clean_d_Square/error_filtered_Square);
}
void compute_psd(float *data, float *x)
{
// twiddle factor is generated off-line
// w is the twiddle factor array
int i, n, j, k;
float rtemp, itemp;
for(i = 0; i < N; i++ )
{
x[2 * i] = data[i];
x[2 * i + 1] = 0;
}
n = N>>1;
j = 0;
for(i=1; i < (n-1); i++)
{
k = n >> 1;
while(k <= j)
{
j -= k;
k >>= 1;
}
j += k;
if(i < j)
{
rtemp = w[j * 2];
w[j * 2] = w[i * 2];
w[i * 2] = rtemp;
itemp = w[j * 2 + 1];
w[j * 2 + 1] = w[i * 2 + 1];
w[i * 2 + 1] = itemp;
}
}
// calculates the fft, the output is in reverse order
DSPF_sp_cfftr2_dit(x, w, N);
// reverse the computed fft coefficients
// bitrev_index(table, N); // table is created off-line as given
above
DSPF_sp_bitrev_cplx ((double*)x, table, N);
}
Check Out Industry's First Single-Chip, Multi-Format, Real-Time HD Video
Transcoding Solution for Commercial & Consumer End Equipment: www.ti.com/dm6467
