digitalFilter.cpp
Go to the documentation of this file.00001 /* 00002 * ---------------------------------------------------------------------------- 00003 * "THE BEER-WARE LICENSE" (Revision 42): 00004 * <joeromano@gmail.com> wrote this file. As long as you retain this notice you 00005 * can do whatever you want with this stuff. If we meet some day, and you think 00006 * this stuff is worth it, you can buy me a beer in return 00007 * Joe Romano and Will McMahan 00008 * ---------------------------------------------------------------------------- 00009 */ 00010 //@author Joe Romano 00011 //@author Will McMahan 00012 //@email joeromano@gmail.com 00013 //@brief digitalFilter.cpp - class to create IIR and FIR digital filter 00014 // coefficients in the Matlab (2010) style. This style being vectors 00015 // of coefficients for the numberator (b) and denominator (a) 00016 // respectively. 00017 // Please refer to the matlab documentation page for implementation 00018 // details: http://www.mathworks.com/access/helpdesk/help/techdoc/ref/filter.html 00019 00020 #include "pr2_gripper_sensor_controller/digitalFilter.h" 00021 00022 digitalFilter::digitalFilter(int filterOrder_userdef, bool isIIR) 00023 { 00024 filterOrder = filterOrder_userdef; 00025 IIR = isIIR; 00026 00027 b = new float [filterOrder + 1]; 00028 a = new float [filterOrder + 1]; 00029 00030 x = new float [filterOrder + 1]; 00031 u = new float [filterOrder + 1]; 00032 00033 // Initialize the arrays with zeros 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, float *b_userdef, float *a_userdef) 00044 { 00045 00046 filterOrder = filterOrder_userdef; 00047 IIR = isIIR; 00048 00049 b = new float [filterOrder + 1]; 00050 a = new float [filterOrder + 1]; 00051 00052 x = new float [filterOrder + 1]; 00053 u = new float [filterOrder + 1]; 00054 00055 // Initialize the arrays 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 float digitalFilter::getNextFilteredValue(float u_current) 00068 { 00069 /* Shift x2 and u2 vectors, losing the last elements and putting new u2 value in zeroth spot. */ 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 /* Simulate system. */ 00077 float output = b[0] * u[0]; 00078 00079 // if we have an IIR filter 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 // if we have an FIR filter 00088 else 00089 { 00090 for (int i = 1 ; i < (filterOrder+1) ; i++) { 00091 output += b[i] * u[i]; 00092 } 00093 } 00094 00095 /* Put the result in shared memory and in the x2 vector. */ 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 }