1 // digitalfilter.cxx - a selection of digital filters
3 // Written by Torsten Dreyer
4 // Based heavily on work created by Curtis Olson, started January 2004.
6 // Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
7 // Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
9 // This program is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU General Public License as
11 // published by the Free Software Foundation; either version 2 of the
12 // License, or (at your option) any later version.
14 // This program is distributed in the hope that it will be useful, but
15 // WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 // General Public License for more details.
19 // You should have received a copy of the GNU General Public License
20 // along with this program; if not, write to the Free Software
21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
24 #include "digitalfilter.hxx"
25 #include "functor.hxx"
33 namespace FGXMLAutopilot {
39 class DigitalFilterImplementation : public SGReferenced {
41 virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0;
43 DigitalFilterImplementation();
44 virtual void initialize( double output ) {}
45 virtual double compute( double dt, double input ) = 0;
46 bool configure( SGPropertyNode_ptr configNode );
48 void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
51 DigitalFilter * _digitalFilter;
54 /* --------------------------------------------------------------------------------- */
55 /* --------------------------------------------------------------------------------- */
56 class GainFilterImplementation : public DigitalFilterImplementation {
58 InputValueList _gainInput;
59 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
61 GainFilterImplementation() : _gainInput(1.0) {}
62 double compute( double dt, double input );
65 class ReciprocalFilterImplementation : public GainFilterImplementation {
67 double compute( double dt, double input );
70 class DerivativeFilterImplementation : public GainFilterImplementation {
71 InputValueList _TfInput;
73 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
75 DerivativeFilterImplementation();
76 double compute( double dt, double input );
79 class ExponentialFilterImplementation : public GainFilterImplementation {
81 InputValueList _TfInput;
82 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
84 double output_1, output_2;
86 ExponentialFilterImplementation();
87 double compute( double dt, double input );
88 virtual void initialize( double output );
91 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
93 InputValueList _samplesInput;
95 std::deque <double> _inputQueue;
96 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
98 MovingAverageFilterImplementation();
99 double compute( double dt, double input );
100 virtual void initialize( double output );
103 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
106 InputValueList _rateOfChangeInput;
107 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
109 NoiseSpikeFilterImplementation();
110 double compute( double dt, double input );
111 virtual void initialize( double output );
114 /* --------------------------------------------------------------------------------- */
115 /* --------------------------------------------------------------------------------- */
117 } // namespace FGXMLAutopilot
119 using namespace FGXMLAutopilot;
121 /* --------------------------------------------------------------------------------- */
122 /* --------------------------------------------------------------------------------- */
123 DigitalFilterImplementation::DigitalFilterImplementation() :
128 bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
130 for (int i = 0; i < configNode->nChildren(); ++i ) {
131 SGPropertyNode_ptr prop;
133 SGPropertyNode_ptr child = configNode->getChild(i);
134 string cname(child->getName());
136 if( configure( cname, child ) )
139 } // for configNode->nChildren()
144 /* --------------------------------------------------------------------------------- */
145 /* --------------------------------------------------------------------------------- */
147 double GainFilterImplementation::compute( double dt, double input )
149 return _gainInput.get_value() * input;
152 bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
154 if (nodeName == "gain" ) {
155 _gainInput.push_back( new InputValue( configNode, 1 ) );
162 /* --------------------------------------------------------------------------------- */
163 /* --------------------------------------------------------------------------------- */
165 double ReciprocalFilterImplementation::compute( double dt, double input )
167 if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
168 return SGLimitsd::max();
170 return _gainInput.get_value() / input;
174 /* --------------------------------------------------------------------------------- */
175 /* --------------------------------------------------------------------------------- */
177 DerivativeFilterImplementation::DerivativeFilterImplementation() :
182 bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
184 if( GainFilterImplementation::configure( nodeName, configNode ) )
187 if (nodeName == "filter-time" ) {
188 _TfInput.push_back( new InputValue( configNode, 1 ) );
195 double DerivativeFilterImplementation::compute( double dt, double input )
197 double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
203 /* --------------------------------------------------------------------------------- */
204 /* --------------------------------------------------------------------------------- */
206 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
211 void MovingAverageFilterImplementation::initialize( double output )
216 double MovingAverageFilterImplementation::compute( double dt, double input )
218 std::deque<double>::size_type samples = _samplesInput.get_value();
219 _inputQueue.resize(samples+1, 0.0);
221 double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
223 _output_1 = output_0;
224 _inputQueue.push_front(input);
228 bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
230 if (nodeName == "samples" ) {
231 _samplesInput.push_back( new InputValue( configNode, 1 ) );
238 /* --------------------------------------------------------------------------------- */
239 /* --------------------------------------------------------------------------------- */
241 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
246 void NoiseSpikeFilterImplementation::initialize( double output )
251 double NoiseSpikeFilterImplementation::compute( double dt, double input )
253 double delta = input - _output_1;
254 if( delta == 0.0 ) return input; // trivial
256 double maxChange = _rateOfChangeInput.get_value() * dt;
257 const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
258 if( periodical ) delta = periodical->normalizeSymmetric( delta );
260 if( fabs(delta) <= maxChange )
261 return (_output_1 = input);
263 return (_output_1 = _output_1 + copysign( maxChange, delta ));
266 bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
268 if (nodeName == "max-rate-of-change" ) {
269 _rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
276 /* --------------------------------------------------------------------------------- */
277 /* --------------------------------------------------------------------------------- */
279 ExponentialFilterImplementation::ExponentialFilterImplementation()
280 : _isSecondOrder(false),
286 void ExponentialFilterImplementation::initialize( double output )
288 output_1 = output_2 = output;
291 double ExponentialFilterImplementation::compute( double dt, double input )
293 input = GainFilterImplementation::compute( dt, input );
294 double tf = _TfInput.get_value();
298 // avoid negative filter times
299 // and div by zero if -tf == dt
301 double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
304 output_0 = alpha * alpha * input +
305 2 * (1 - alpha) * output_1 -
306 (1 - alpha) * (1 - alpha) * output_2;
308 output_0 = alpha * input + (1 - alpha) * output_1;
311 return (output_1 = output_0);
314 bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
316 if( GainFilterImplementation::configure( nodeName, configNode ) )
319 if (nodeName == "filter-time" ) {
320 _TfInput.push_back( new InputValue( configNode, 1 ) );
324 if (nodeName == "type" ) {
325 string type(configNode->getStringValue());
326 _isSecondOrder = type == "double-exponential";
332 /* --------------------------------------------------------------------------------- */
333 /* Digital Filter Component Implementation */
334 /* --------------------------------------------------------------------------------- */
336 DigitalFilter::DigitalFilter() :
338 _initializeTo(INITIALIZE_INPUT)
342 DigitalFilter::~DigitalFilter()
347 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
349 bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
351 if( componentForge.empty() ) {
352 componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
353 componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
354 componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
355 componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
356 componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
357 componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
358 componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
361 SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
362 if( AnalogComponent::configure( nodeName, configNode ) )
365 if (nodeName == "type" ) {
366 string type( configNode->getStringValue() );
367 if( componentForge.count(type) == 0 ) {
368 SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
371 _implementation = (*componentForge[type])( configNode->getParent() );
372 _implementation->setDigitalFilter( this );
376 if( nodeName == "initialize-to" ) {
377 string s( configNode->getStringValue() );
379 _initializeTo = INITIALIZE_INPUT;
380 } else if( s == "output" ) {
381 _initializeTo = INITIALIZE_OUTPUT;
382 } else if( s == "none" ) {
383 _initializeTo = INITIALIZE_NONE;
385 SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
390 SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
391 return false; // not handled by us, let the base class try
394 void DigitalFilter::update( bool firstTime, double dt)
396 if( _implementation == NULL ) return;
399 switch( _initializeTo ) {
401 case INITIALIZE_INPUT:
402 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
403 _implementation->initialize( _valueInput.get_value() );
406 case INITIALIZE_OUTPUT:
407 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
408 _implementation->initialize( get_output_value() );
412 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
417 double input = _valueInput.get_value() - _referenceInput.get_value();
418 double output = _implementation->compute( dt, input );
420 set_output_value( output );
423 cout << "input:" << input
424 << "\toutput:" << output << endl;