Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "collision_detection/digitalFilter.h"
00021
00022 digitalFilter::digitalFilter(int filterOrder_userdef, bool isIIR)
00023 {
00024 filterOrder = filterOrder_userdef;
00025 IIR = isIIR;
00026
00027 b = new double [filterOrder + 1];
00028 a = new double [filterOrder + 1];
00029
00030 x = new double [filterOrder + 1];
00031 u = new double [filterOrder + 1];
00032
00033
00034 for(int i = 0; i < (filterOrder + 1); i++)
00035 {
00036 b[i] = 0.0;
00037 a[i] = 0.0;
00038 x[i] = 0.0;
00039 u[i] = 0.0;
00040 }
00041 }
00042
00043 digitalFilter::digitalFilter(int filterOrder_userdef, bool isIIR, double *b_userdef, double *a_userdef)
00044 {
00045
00046 filterOrder = filterOrder_userdef;
00047 IIR = isIIR;
00048
00049 b = new double [filterOrder + 1];
00050 a = new double [filterOrder + 1];
00051
00052 x = new double [filterOrder + 1];
00053 u = new double [filterOrder + 1];
00054
00055
00056
00057 for(int i = 0; i < (filterOrder + 1); i++)
00058 {
00059 b[i] = b_userdef[i];
00060 a[i] = a_userdef[i];
00061 x[i] = 0.0;
00062 u[i] = 0.0;
00063 }
00064
00065 }
00066
00067 double digitalFilter::getNextFilteredValue(double u_current)
00068 {
00069
00070 for (int i = filterOrder ; i > 0 ; i--) {
00071 x[i] = x[i-1];
00072 u[i] = u[i-1];
00073 }
00074 u[0] = u_current;
00075
00076
00077 double output = b[0] * u[0];
00078
00079
00080 if(IIR)
00081 {
00082 for (int i = 1 ; i < (filterOrder+1) ; i++) {
00083 output += b[i] * u[i] - a[i] * x[i];
00084 }
00085 }
00086
00087
00088 else
00089 {
00090 for (int i = 1 ; i < (filterOrder+1) ; i++) {
00091 output += b[i] * u[i];
00092 }
00093 }
00094
00095
00096 x[0] = output;
00097
00098 return output;
00099 }
00100
00101 digitalFilter::~digitalFilter(void)
00102 {
00103 delete[] x;
00104 delete[] u;
00105 delete[] a;
00106 delete[] b;
00107 }