PosVelFilter.cpp

00001 #include "PosVelFilter.hpp"
00002 //#include "StaticDefs.hpp"
00003 
00004 PosVelFilter::PosVelFilter() {  // if heading_flag is set, operate on sine and cosine, return atan2(sin, cos) as postion and velocity??
00005     x1 = 0; // pseudo position state
00006     x2 = 0; // pseudo velocity state
00007     x1c = 0;
00008     x2c = 0;
00009     x1s = 0;
00010     x2s = 0;
00011     w_n = 1.0; // natural frequency of the filter:   bigger increases frequency response
00012     _headingFlag = false;
00013 }
00014 
00015 //run the pos-vel estimate filter
00016 void PosVelFilter::update(float deltaT, float counts) {
00017     dt = deltaT;
00018     
00019     if(!_headingFlag) {
00020       x1_dot = x2;
00021       x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
00022 
00023       position = x1;
00024       velocity = x2;
00025 
00026       x1 += x1_dot*dt;
00027       x2 += x2_dot*dt;
00028     }
00029     if(_headingFlag) {
00030         counts_c = cos(counts * _PI/180.0);  // math.h cos and sine operate on radians , counts is heading degrees
00031         counts_s = sin(counts * _PI/180.0);
00032         // the cosine part
00033         x1c_dot = x2c;
00034         x2c_dot = (-2.0*w_n*x2c) - (w_n*w_n)*x1c + (w_n*w_n)*counts_c;
00035 
00036         position_c = x1c;
00037         velocity_c = x2c;
00038 
00039         x1c += x1c_dot*dt;
00040         x2c += x2c_dot*dt;
00041           //   now the sine portion
00042         x1s_dot = x2s;
00043         x2s_dot = (-2.0*w_n*x2s) - (w_n*w_n)*x1s + (w_n*w_n)*counts_s;
00044 
00045         position_s = x1s;
00046         velocity_s = x2s;
00047 
00048         x1s += x1s_dot*dt;
00049         x2s += x2s_dot*dt;
00050       // now reconstitute the angle and the angular velocity
00051         position = atan2(position_s, position_c)  * 180.0/_PI;
00052         pnew = atan2(x1s,x1c) * 180/_PI;  // the next value, since the update has happened by now.
00053         velocity = (pnew - position) ;  // don't multiply by dt yet, now just an angular difference
00054         if(position < 0 ) { position = position +360; }
00055         if(velocity  > 180 ) { velocity -= 360;  }
00056         if(velocity <= -180)  { velocity += 360 ;}
00057         velocity *= (1.0/dt);
00058     }
00059 }
00060 
00061 float PosVelFilter::getPosition() {
00062     return position;
00063 }
00064 
00065 float PosVelFilter::getVelocity() {
00066     return velocity;
00067 }
00068 void PosVelFilter::setHeadingFlag(bool heading_flag) {
00069     _headingFlag = heading_flag;
00070 }
00071 
00072     
00073 
00074 float PosVelFilter::getDt() {
00075     return dt;
00076 }
00077 
00078 void PosVelFilter::writeWn(float wn) {
00079     w_n = wn;
00080 }