main()
{
/* test program for above utility routines */
double **a, **b, **c, **bT;
double *x, *y, *z;
FILE *infile, *outfile;
int a_rows, a_cols, b_rows, b_cols, errors, xn, yn;
infile = fopen("mat.in", "r");
outfile = fopen("mat.dat", "w");
a = dReadMatrix( infile, &a_rows, &a_cols, &errors);
b = dReadMatrix( infile, &b_rows, &b_cols, &errors);
x = dReadVector( infile, &xn, &errors);
y = dReadVector( infile, &yn, &errors);
getchar();
dmdump( stdout, "Matrix A", a, a_rows, a_cols, "%8.2lf");
dmdump( stdout, "Matrix B", b, b_rows, b_cols, "%8.2lf");
dvdump( stdout, "Vector x", x, xn, "%8.2lf");
dvdump( stdout, "Vector y", y, yn, "%8.2lf");
z = dvector( 1, xn );
dvadd( x, xn, y, z );
dvdump( stdout, "x + y", z, xn, "%8.2lf");
dvsub( x, xn, y, z );
dvdump( stdout, "x - y", z, xn, "%8.2lf");
dvsmy( x, xn, 2.0, z );
dvdump( stdout, "2x", z, xn, "%8.2lf");
printf("Magnitude of 2x: %7.2lf\n", dvmag( z, xn ));
printf("dot product x.y: %7.2lf\n", dvdot( x, xn, y));
dmvmult( a, a_rows, a_cols, x, xn, z );
dvdump( stdout, "Ax", z, xn, "%8.2lf");
c = dmatrix( 1, a_rows, 1, b_cols );
bT = dmatrix( 1, b_cols, 1, b_rows );
dmtranspose( b, b_rows, b_cols, bT);
dmdump( stdout, "Matrix B (transposed)", bT, b_cols, b_rows, "%8.2lf");
dmmult( a, a_rows, a_cols, bT, b_cols, b_rows, c);
dmdump( stdout, "Matrix AB", c, a_rows, b_rows, "%8.2lf");
/* dmfillUT( a, a_rows, a_cols );
dmdump( stdout, "Symmetrified matrix A", a, a_rows, a_cols, "%8.2lf"); */
free_dmatrix( a, 1, a_rows, 1, a_cols);
free_dmatrix( b, 1, b_rows, 1, b_cols);
free_dmatrix( c, 1, a_rows, 1, b_cols);
free_dvector( x, 1, xn );
free_dvector( y, 1, yn );
}
void vad(float rc,
float * lsf,
float * rxx,
float * sigpp,
int frm_count,
int prev_marker, int pprev_marker, int *marker, float * Energy_db)
{
float tmp[M];
float SD;
float E_low;
float dtemp;
float dSE;
float dSLE;
float ZC;
float COEF;
float COEFZC;
float COEFSD;
float dSZC;
float norm_energy;
int i;
/* compute the frame energy */
norm_energy =
(float) 10.0 *(float) log10((float) (rxx[0] / (float) 240.0 + EPSI));
*Energy_db = norm_energy;
/* compute the low band energy */
E_low = (float) 0.0;
for (i = 1; i <= NP; i++)
E_low = E_low + rxx[i] * lbf_corr[i];
E_low = rxx[0] * lbf_corr[0] + (float) 2.0 *E_low;
if (E_low < (float) 0.0)
E_low = (float) 0.0;
E_low = (float) 10.0 *(float) log10((float) (E_low / (float) 240.0 + EPSI));
/* compute SD */
/* Normalize lsfs */
for (i = 0; i < M; i++)
lsf[i] /= (float) 2. *PI;
dvsub(lsf, MeanLSF, tmp, M);
SD = dvdot(tmp, tmp, M);
/* compute # zero crossing */
ZC = (float) 0.0f;
dtemp = sigpp[ZC_START];
for (i = ZC_START + 1; i <= ZC_END; i++) {
if (dtemp * sigpp[i] < (float) 0.0) {
ZC = ZC + (float) 1.0;
}
dtemp = sigpp[i];
}
ZC = ZC / (float) 80.0;
/* Initialize and update Mins */
if (frm_count < 129) {
if (norm_energy < Min) {
Min = norm_energy;
Prev_Min = norm_energy;
}
if ((frm_count % 8) == 0) {
Min_buffer[(int)frm_count / 8 - 1] = Min;
Min = FLT_MAX_G729;
}
}
if ((frm_count % 8) == 0) {
Prev_Min = Min_buffer[0];
for (i = 1; i < 15; i++) {
if (Min_buffer[i] < Prev_Min)
Prev_Min = Min_buffer[i];
}
}
if (frm_count >= 129) {
if ((frm_count % 8) == 1) {
Min = Prev_Min;
Next_Min = FLT_MAX_G729;
}
if (norm_energy < Min)
Min = norm_energy;
if (norm_energy < Next_Min)
Next_Min = norm_energy;
if ((frm_count % 8) == 0) {
for (i = 0; i < 15; i++)
Min_buffer[i] = Min_buffer[i + 1];
Min_buffer[15] = Next_Min;
Prev_Min = Min_buffer[0];
for (i = 1; i < 16; i++) {
if (Min_buffer[i] < Prev_Min)
Prev_Min = Min_buffer[i];
}
}
}
if (frm_count <= INIT_FRAME) {
if (norm_energy < (float) 21.0) {
less_count++;
*marker = NOISE;
} else {
*marker = VOICE;
MeanE =
(MeanE * ((float) (frm_count - less_count - 1)) +
norm_energy) / (float) (frm_count - less_count);
MeanSZC =
(MeanSZC * ((float) (frm_count - less_count - 1)) +
ZC) / (float) (frm_count - less_count);
dvwadd(MeanLSF, (float) (frm_count - less_count - 1),
lsf, (float) 1.0, MeanLSF, M);
dvsmul(MeanLSF,
(float) 1.0 / (float) (frm_count - less_count),
MeanLSF, M);
}
}
if (frm_count >= INIT_FRAME) {
if (frm_count == INIT_FRAME) {
MeanSE = MeanE - (float) 10.0;
MeanSLE = MeanE - (float) 12.0;
}
dSE = MeanSE - norm_energy;
dSLE = MeanSLE - E_low;
dSZC = MeanSZC - ZC;
if (norm_energy < (float) 21.0) {
*marker = NOISE;
} else {
*marker = MakeDec(dSLE, dSE, SD, dSZC);
}
v_flag = 0;
if ((prev_marker == VOICE) && (*marker == NOISE) &&
(norm_energy > MeanSE + (float) 2.0)
&& (norm_energy > (float) 21.0)) {
*marker = VOICE;
v_flag = 1;
}
if (flag == 1) {
if ((pprev_marker == VOICE) && (prev_marker == VOICE) &&
(*marker == NOISE)
&& (fabs(prev_energy - norm_energy) <= (float) 3.0)) {
count_ext++;
*marker = VOICE;
v_flag = 1;
if (count_ext <= 4)
flag = 1;
else {
flag = 0;
count_ext = 0;
}
}
} else
flag = 1;
if (*marker == NOISE)
count_sil++;
if ((*marker == VOICE) && (count_sil > 10) &&
((norm_energy - prev_energy) <= (float) 3.0)) {
*marker = NOISE;
count_sil = 0;
}
if (*marker == VOICE)
count_sil = 0;
if ((norm_energy < MeanSE + (float) 3.0) && (frm_count > 128)
&& (!v_flag) && (rc < (float) 0.6))
*marker = NOISE;
if ((norm_energy < MeanSE + (float) 3.0) && (rc < (float) 0.75)
&& (SD < (float) 0.002532959)) {
count_update++;
if (count_update < INIT_COUNT) {
COEF = (float) 0.75;
COEFZC = (float) 0.8;
COEFSD = (float) 0.6;
} else if (count_update < INIT_COUNT + 10) {
COEF = (float) 0.95;
COEFZC = (float) 0.92;
COEFSD = (float) 0.65;
} else if (count_update < INIT_COUNT + 20) {
COEF = (float) 0.97;
COEFZC = (float) 0.94;
COEFSD = (float) 0.70;
} else if (count_update < INIT_COUNT + 30) {
COEF = (float) 0.99;
COEFZC = (float) 0.96;
COEFSD = (float) 0.75;
} else if (count_update < INIT_COUNT + 40) {
COEF = (float) 0.995;
COEFZC = (float) 0.99;
COEFSD = (float) 0.75;
} else {
COEF = (float) 0.995;
COEFZC = (float) 0.998;
COEFSD = (float) 0.75;
}
dvwadd(MeanLSF, COEFSD, lsf, (float) 1.0 - COEFSD, MeanLSF,
M);
MeanSE = COEF * MeanSE + ((float) 1.0 - COEF) * norm_energy;
MeanSLE = COEF * MeanSLE + ((float) 1.0 - COEF) * E_low;
MeanSZC = COEFZC * MeanSZC + ((float) 1.0 - COEFZC) * ZC;
}
if (((frm_count > 128)
&& ((MeanSE < Min) && (SD < (float) 0.002532959)))
|| (MeanSE > Min + (float) 10.0)) {
MeanSE = Min;
count_update = 0;
}
}
prev_energy = norm_energy;
return;
}
void vad(struct vad_state_t * state,
GFLOAT rc,
GFLOAT *lsf,
GFLOAT *rxx,
GFLOAT *sigpp,
int frm_count,
int prev_marker,
int pprev_marker,
int *marker,
GFLOAT *Energy_db)
{
GFLOAT tmp[M];
GFLOAT SD;
GFLOAT E_low;
GFLOAT dtemp;
GFLOAT dSE;
GFLOAT dSLE;
GFLOAT ZC;
GFLOAT COEF;
GFLOAT COEFZC;
GFLOAT COEFSD;
GFLOAT dSZC;
GFLOAT norm_energy;
int i;
/* compute the frame energy */
norm_energy = (F)10.0*(GFLOAT) log10((GFLOAT)( rxx[0]/(F)240.0 +EPSI));
*Energy_db = norm_energy ;
/* compute the low band energy */
E_low = (F)0.0;
for( i=1; i<= NP; i++)
E_low = E_low + rxx[i]*lbf_corr[i];
E_low= rxx[0]*lbf_corr[0] + (F)2.0*E_low;
if (E_low < (F)0.0) E_low = (F)0.0;
E_low= (F)10.0*(GFLOAT) log10((GFLOAT) (E_low/(F)240.0+EPSI));
/* compute SD */
/* Normalize lsfs */
for(i=0; i<M; i++) lsf[i] /= (F)2.*PI;
dvsub(lsf, state->MeanLSF,tmp,M);
SD = dvdot(tmp,tmp,M);
/* compute # zero crossing */
ZC = (F)0.0f;
dtemp = sigpp[ZC_START];
for (i=ZC_START+1 ; i <= ZC_END ; i++) {
if (dtemp*sigpp[i] < (F)0.0) {
ZC= ZC +(F)1.0;
}
dtemp = sigpp[i];
}
ZC = ZC/(F)80.0;
/* Initialize and update Mins */
if( frm_count < 129 ) {
if( norm_energy < state->Min ){
state->Min = norm_energy;
state->Prev_Min = norm_energy;
}
if( (frm_count % 8) == 0){
state->Min_buffer[(int)frm_count/8 -1] = state->Min;
state->Min = FLT_MAX_G729;
}
}
if( (frm_count % 8) == 0){
state->Prev_Min = state->Min_buffer[0];
for ( i =1; i< 15; i++){
if ( state->Min_buffer[i] < state->Prev_Min )
state->Prev_Min = state->Min_buffer[i];
}
}
if( frm_count >= 129 ) {
if( (frm_count % 8 ) == 1) {
state->Min = state->Prev_Min;
state->Next_Min = FLT_MAX_G729;
}
if( norm_energy < state->Min )
state->Min = norm_energy;
if( norm_energy < state->Next_Min )
state->Next_Min = norm_energy;
if( (frm_count % 8) == 0){
for ( i =0; i< 15; i++)
state->Min_buffer[i] = state->Min_buffer[i+1];
state->Min_buffer[15] = state->Next_Min;
state->Prev_Min = state->Min_buffer[0];
for ( i =1; i< 16; i++){
if ( state->Min_buffer[i] < state->Prev_Min )
state->Prev_Min = state->Min_buffer[i];
}
}
}
if (frm_count <= INIT_FRAME){
if( norm_energy < (F)21.0){
state->less_count++;
*marker = NOISE;
}
else{
*marker = VOICE;
state->MeanE = (state->MeanE * ( (GFLOAT)(frm_count - state->less_count -1)) +
norm_energy)/(GFLOAT) (frm_count - state->less_count);
state->MeanSZC = (state->MeanSZC * ( (GFLOAT)(frm_count - state->less_count -1)) +
ZC)/(GFLOAT) (frm_count - state->less_count);
dvwadd(state->MeanLSF,(GFLOAT) (frm_count - state->less_count -1),lsf,(F)1.0,state->MeanLSF,M);
dvsmul(state->MeanLSF,(F)1.0/(GFLOAT) (frm_count - state->less_count ), state->MeanLSF,M);
}
}
if (frm_count >= INIT_FRAME ){
if (frm_count == INIT_FRAME ){
state->MeanSE = state->MeanE -(F)10.0;
state->MeanSLE = state->MeanE -(F)12.0;
}
dSE = state->MeanSE - norm_energy;
dSLE = state->MeanSLE - E_low;
dSZC = state->MeanSZC - ZC;
if( norm_energy < (F)21.0 ){
*marker = NOISE;
}
else{
*marker = vad_make_dec(dSLE, dSE, SD, dSZC );
}
state->v_flag =0;
if( (prev_marker == VOICE) && (*marker == NOISE) &&
(norm_energy > state->MeanSE + (F)2.0) && ( norm_energy>(F)21.0)){
*marker = VOICE;
state->v_flag=1;
}
if((state->flag == 1) ){
if( (pprev_marker == VOICE) && (prev_marker == VOICE) &&
(*marker == NOISE) && (fabs(state->prev_energy - norm_energy)<= (F)3.0)){
state->count_ext++;
*marker = VOICE;
state->v_flag=1;
if(state->count_ext <=4)
state->flag =1;
else{
state->flag =0;
state->count_ext=0;
}
}
}
else
state->flag =1;
if(*marker == NOISE)
state->count_sil++;
if((*marker == VOICE) && (state->count_sil > 10) &&
((norm_energy - state->prev_energy) <= (F)3.0)){
*marker = NOISE;
state->count_sil=0;
}
if(*marker == VOICE)
state->count_sil=0;
if ((norm_energy < state->MeanSE + (F)3.0) && ( frm_count >128) &&( !state->v_flag) && (rc <(F)0.6) )
*marker = NOISE;
if ((norm_energy < state->MeanSE + (F)3.0) && (rc <(F)0.75) && ( SD<(F)0.002532959)){
state->count_update++;
if (state->count_update < INIT_COUNT){
COEF = (F)0.75;
COEFZC = (F)0.8;
COEFSD = (F)0.6;
}
else
if (state->count_update < INIT_COUNT+10){
COEF = (F)0.95;
COEFZC = (F)0.92;
COEFSD = (F)0.65;
}
else
if (state->count_update < INIT_COUNT+20){
COEF = (F)0.97;
COEFZC = (F)0.94;
COEFSD = (F)0.70;
}
else
if (state->count_update < INIT_COUNT+30){
COEF = (F)0.99;
COEFZC = (F)0.96;
COEFSD = (F)0.75;
}
else
if (state->count_update < INIT_COUNT+40){
COEF = (F)0.995;
COEFZC = (F)0.99;
COEFSD = (F)0.75;
}
else{
COEF = (F)0.995;
COEFZC = (F)0.998;
COEFSD = (F)0.75;
}
dvwadd(state->MeanLSF,COEFSD,lsf,(F)1.0 -COEFSD,state->MeanLSF,M);
state->MeanSE = COEF * state->MeanSE+((F)1.0- COEF)*norm_energy;
state->MeanSLE = COEF * state->MeanSLE+((F)1.0- COEF)*E_low;
state->MeanSZC = COEFZC * state->MeanSZC+((F)1.0- COEFZC)*ZC;
}
if(((frm_count > 128) && (state->MeanSE < state->Min) && (SD < (F)0.002532959)) || (state->MeanSE > state->Min + (F)10.0) )
{
state->MeanSE = state->Min;
state->count_update = 0;
}
}
state->prev_energy = norm_energy;
return;
}
void musdetect(
int rate,
FLOAT Energy,
FLOAT *rc,
int *lags,
FLOAT *pgains,
int stat_flg,
int frm_count,
int prev_vad,
int *Vad,
FLOAT LLenergy)
{
int i;
static int count_music=0;
static FLOAT Mcount_music=(F)0.0;
static int count_consc=0;
FLOAT sum1, sum2,std;
static FLOAT MeanPgain =(F)0.5;
short PFLAG1, PFLAG2, PFLAG;
static int count_pflag=0;
static FLOAT Mcount_pflag=(F)0.0;
static int count_consc_pflag=0;
static int count_consc_rflag=0;
static FLOAT mrc[10]={(F)0.0,(F)0.0, (F)0.0, (F)0.0, (F)0.0,
(F)0.0, (F)0.0, (F)0.0,(F)0.0,(F)0.0};
static FLOAT MeanSE =(F)0.0;
FLOAT pderr, Lenergy , SD, tmp_vec[10];
FLOAT Thres;
pderr =(F)1.0;
for (i=0; i< 4; i++) pderr *= ((F)1.0 - rc[i]*rc[i]);
dvsub(mrc,rc,tmp_vec,10);
SD = dvdot(tmp_vec, tmp_vec,10);
Lenergy = (F)10.0*(FLOAT)log10(pderr*Energy/(F)240.0 +EPSI);
if( *Vad == NOISE ){
dvwadd(mrc,(F)0.9,rc,(F)0.1,mrc,10);
MeanSE = (F)0.9*MeanSE + (F)0.1*Lenergy;
}
sum1 =(F)0.0;
sum2 =(F)0.0;
for(i=0; i<5; i++){
sum1 += (FLOAT) lags[i];
sum2 += pgains[i];
}
sum1 = sum1/(F)5.0;
sum2 = sum2/(F)5.0;
std =(F)0.0;
for(i=0; i<5; i++) std += sqr(((FLOAT) lags[i] - sum1));
std = (FLOAT)sqrt(std/(F)4.0);
MeanPgain = (F)0.8*MeanPgain + (F)0.2*sum2;
if ( rate == G729D)
Thres = (F)0.73;
else
Thres = (F)0.63;
if ( MeanPgain > Thres)
PFLAG2 =1;
else
PFLAG2 =0;
if ( std < (F)1.30 && MeanPgain > (F)0.45 )
PFLAG1 =1;
else
PFLAG1 =0;
PFLAG= (INT16)( ((INT16)prev_vad & (INT16)(PFLAG1 | PFLAG2))| (INT16)(PFLAG2));
if( rc[1] <= (F)0.45 && rc[1] >= (F)0.0 && MeanPgain < (F)0.5)
count_consc_rflag++;
else
count_consc_rflag =0;
if( stat_flg== 1 && (*Vad == VOICE))
count_music++;
if ((frm_count%64) == 0 ){
if( frm_count == 64)
Mcount_music = (FLOAT)count_music;
else
Mcount_music = (F)0.9*Mcount_music + (F)0.1*(FLOAT)count_music;
}
if( count_music == 0)
count_consc++;
else
count_consc = 0;
if( count_consc > 500 || count_consc_rflag > 150) Mcount_music = (F)0.0;
if ((frm_count%64) == 0)
count_music = 0;
if( PFLAG== 1 )
count_pflag++;
if ((frm_count%64) == 0 ){
if( frm_count == 64)
Mcount_pflag = (FLOAT)count_pflag;
else{
if( count_pflag > 25)
Mcount_pflag = (F)0.98*Mcount_pflag + (F)0.02*(FLOAT)count_pflag;
else if( count_pflag > 20)
Mcount_pflag = (F)0.95*Mcount_pflag + (F)0.05*(FLOAT)count_pflag;
else
Mcount_pflag = (F)0.90*Mcount_pflag + (F)0.10*(FLOAT)count_pflag;
}
}
if( count_pflag == 0)
count_consc_pflag++;
else
count_consc_pflag = 0;
if( count_consc_pflag > 100 || count_consc_rflag > 150) Mcount_pflag = (F)0.0;
if ((frm_count%64) == 0)
count_pflag = 0;
if (rate == G729E){
if( SD > (F)0.15 && (Lenergy -MeanSE)> (F)4.0 && (LLenergy> 50.0) )
*Vad =VOICE;
else if( (SD > (F)0.38 || (Lenergy -MeanSE)> (F)4.0 ) && (LLenergy> 50.0))
*Vad =VOICE;
else if( (Mcount_pflag >= (F)10.0 || Mcount_music >= (F)5.0 || frm_count < 64)
&& (LLenergy> 7.0))
*Vad =VOICE;
}
return;
}