#include "IIR_pade.h"
#include "math.h"
double D[8][2]={{0}};
/**Calculates the IIR coefficients, using the Pade Method,
steps:
1. calculates the FIR coefficients
2. Creates a Toeplitz augmented matrix from the FIR coefficients avoiding the
first one and the last one on the left side of the matrix, the right side is filled with
a diagonal of ones (Identity matrix)
3. Then the we get inverse of this matrix using a simple gauss jordan algorithm
4. Get the dot product between Inverse[Toeplitz] and the las elements of the
FIR vector, this results in the denominator coefficients of the transfer function
5. Create a Lower Triangular Matrix using a pattern of the FIR coefficients
6. We multiply this matrix with a vector with the solution vector from step 4, getting
the numerator coefficients**/
IIR_pade::IIR_pade(double alpha,int padeOrder,double approximator,int Ts,double num[],double den[]) {
_padeOrder = padeOrder;
*_num = *num;
*_den = *den;
_Gain = pow(1/Ts,_padeOrder);
int L = (2*padeOrder)+1;
double h[15]={0.0};
/*FIR coefficients*/
for(int n = 0; n < L; n++){
for(int k = 0; k <= n; k++){
h[n] += pow(approximator,-alpha-n+k)*pow(1.0-approximator,n-k)*
Bi_E(alpha,k)*Bi(-alpha,n-k);
}
}
int LM = 2*padeOrder;
double H2[7][14]={0.0};
double temp=0.0;
/*Toeplitz augmented matrix*/
for (int i = 0; i < padeOrder; i++) {
for (int j = 0; j < LM; j++) {
if (j == (i + padeOrder)){H2[i][j] = 1.0;}
else {H2[i][j] = 0.0;}
if(j<padeOrder) {H2[i][j] = h[i-j+padeOrder];}
}
}
/*start the inverse of the Toeplitz matrix*/
for (int i = padeOrder - 1; i > 0; i--) {
if (H2[i - 1][0] < H2[i][0])
for (int j = 0; j < LM; j++) {
temp = H2[i][j];
H2[i][j] = H2[i - 1][j];
H2[i - 1][j] = temp;
}
}
for (int i = 0; i < padeOrder; i++) {
for (int j = 0; j < padeOrder; j++) {
if (j != i) {
temp = H2[j][i] / H2[i][i];
for (int k = 0; k < LM; k++){H2[j][k] -= H2[i][k] * temp;}
}
}
}
for (int i = 0; i < padeOrder; i++) {
temp = H2[i][i];
for (int j = 0; j < LM; j++){H2[i][j] = H2[i][j] / temp;}
}
/*ends the inverse of Toeplitz*/
/*First dot product - denominator coefficients*/
den[0] = 1.0;
for(int i=0;i<padeOrder;i++){
for(int j=0;j<padeOrder;j++){den[i+1] -= H2[i][j+padeOrder]*h[j+padeOrder+1];}
}
double H1[8][8]={0.0};
/*Lower triangular matrix*/
for(int i = 0; i <=padeOrder; i++){
for(int j = 0; j <= padeOrder; j++){
if(j<=i){H1[i][j] = h[i-j];}
else{H1[i][j] = 0.0;}
}
}
/*second dot product - numerator coefficients*/
for(int i=0;i<=padeOrder;i++){
for(int j=0;j<=padeOrder;j++){num[i] += H1[i][j]*den[j];}
}
}
/*IIR filter*/
double IIR_pade::Filter(double input){
double output=0;
output = input*_num[0]*_Gain + D[0][1];
if(_padeOrder>1)
for(int j=0;j<_padeOrder-1;j++) D[j][0] = _num[j+1]*input*_Gain - _den[j+1]*output + D[j+1][1];
D[_padeOrder-1][0] = _num[_padeOrder]*input*_Gain-_den[_padeOrder]*output;
for(int x=0;x<_padeOrder;x++) D[x][1] = D[x][0];
return output;
}
/*recursive binomial*/
double IIR_pade::Bi_E(double a,int n){
if (n==0) return 1.0;
else return (1.0-(1.0+a)/n)*Bi_E(a,n-1);
}
/*recursive binomial*/
double IIR_pade::Bi(double a,int n){
if (n==0) return 1.0;
else return ((a+1.0)/n - 1.0)*Bi(a,n-1);
}
