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 // Washout/high-pass filter, lead-lag filter and integrator added.
10 // low-pass and lag aliases added to Exponential filter,
11 // rate-limit added. A J Teeder 2013
13 // This program is free software; you can redistribute it and/or
14 // modify it under the terms of the GNU General Public License as
15 // published by the Free Software Foundation; either version 2 of the
16 // License, or (at your option) any later version.
18 // This program is distributed in the hope that it will be useful, but
19 // WITHOUT ANY WARRANTY; without even the implied warranty of
20 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
21 // General Public License for more details.
23 // You should have received a copy of the GNU General Public License
24 // along with this program; if not, write to the Free Software
25 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
28 #include "digitalfilter.hxx"
31 namespace FGXMLAutopilot
38 class DigitalFilterImplementation:
42 virtual ~DigitalFilterImplementation() {}
43 DigitalFilterImplementation();
44 virtual void initialize( double initvalue ) {}
45 virtual double compute( double dt, double input ) = 0;
46 virtual bool configure( SGPropertyNode& cfg_node,
47 const std::string& cfg_name,
48 SGPropertyNode& prop_root ) = 0;
50 void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
53 DigitalFilter * _digitalFilter;
56 /* --------------------------------------------------------------------------------- */
57 /* --------------------------------------------------------------------------------- */
58 class GainFilterImplementation : public DigitalFilterImplementation {
60 InputValueList _gainInput;
61 bool configure( SGPropertyNode& cfg_node,
62 const std::string& cfg_name,
63 SGPropertyNode& prop_root );
65 GainFilterImplementation() : _gainInput(1.0) {}
66 double compute( double dt, double input );
69 class ReciprocalFilterImplementation : public GainFilterImplementation {
71 double compute( double dt, double input );
74 class DerivativeFilterImplementation : public GainFilterImplementation {
75 InputValueList _TfInput;
77 bool configure( SGPropertyNode& cfg_node,
78 const std::string& cfg_name,
79 SGPropertyNode& prop_root );
81 DerivativeFilterImplementation();
82 double compute( double dt, double input );
83 virtual void initialize( double initvalue );
86 class ExponentialFilterImplementation : public GainFilterImplementation {
88 InputValueList _TfInput;
89 bool configure( SGPropertyNode& cfg_node,
90 const std::string& cfg_name,
91 SGPropertyNode& prop_root );
93 double _output_1, _output_2;
95 ExponentialFilterImplementation();
96 double compute( double dt, double input );
97 virtual void initialize( double initvalue );
100 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
102 InputValueList _samplesInput;
104 std::deque <double> _inputQueue;
105 bool configure( SGPropertyNode& cfg_node,
106 const std::string& cfg_name,
107 SGPropertyNode& prop_root );
109 MovingAverageFilterImplementation();
110 double compute( double dt, double input );
111 virtual void initialize( double initvalue );
114 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
117 InputValueList _rateOfChangeInput;
118 bool configure( SGPropertyNode& cfg_node,
119 const std::string& cfg_name,
120 SGPropertyNode& prop_root );
122 NoiseSpikeFilterImplementation();
123 double compute( double dt, double input );
124 virtual void initialize( double initvalue );
127 class RateLimitFilterImplementation : public DigitalFilterImplementation {
130 InputValueList _rateOfChangeMax;
131 InputValueList _rateOfChangeMin ;
132 bool configure( SGPropertyNode& cfg_node,
133 const std::string& cfg_name,
134 SGPropertyNode& prop_root );
136 RateLimitFilterImplementation();
137 double compute( double dt, double input );
138 virtual void initialize( double initvalue );
141 class IntegratorFilterImplementation : public GainFilterImplementation {
143 InputValueList _TfInput;
144 InputValueList _minInput;
145 InputValueList _maxInput;
148 bool configure( SGPropertyNode& cfg_node,
149 const std::string& cfg_name,
150 SGPropertyNode& prop_root );
152 IntegratorFilterImplementation();
153 double compute( double dt, double input );
154 virtual void initialize( double initvalue );
157 class HighPassFilterImplementation : public GainFilterImplementation {
159 InputValueList _TfInput;
162 bool configure( SGPropertyNode& cfg_node,
163 const std::string& cfg_name,
164 SGPropertyNode& prop_root );
166 HighPassFilterImplementation();
167 double compute( double dt, double input );
168 virtual void initialize( double initvalue );
170 class LeadLagFilterImplementation : public GainFilterImplementation {
172 InputValueList _TfaInput;
173 InputValueList _TfbInput;
176 bool configure( SGPropertyNode& cfg_node,
177 const std::string& cfg_name,
178 SGPropertyNode& prop_root );
180 LeadLagFilterImplementation();
181 double compute( double dt, double input );
182 virtual void initialize( double initvalue );
184 /* --------------------------------------------------------------------------------- */
185 /* --------------------------------------------------------------------------------- */
187 } // namespace FGXMLAutopilot
189 using namespace FGXMLAutopilot;
191 //------------------------------------------------------------------------------
192 DigitalFilterImplementation::DigitalFilterImplementation() :
198 //------------------------------------------------------------------------------
199 double GainFilterImplementation::compute( double dt, double input )
201 return _gainInput.get_value() * input;
204 bool GainFilterImplementation::configure( SGPropertyNode& cfg_node,
205 const std::string& cfg_name,
206 SGPropertyNode& prop_root )
208 if (cfg_name == "gain" ) {
209 _gainInput.push_back( new InputValue(prop_root, cfg_node, 1) );
216 /* --------------------------------------------------------------------------------- */
217 /* --------------------------------------------------------------------------------- */
219 double ReciprocalFilterImplementation::compute( double dt, double input )
221 if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
222 return SGLimitsd::max();
224 return _gainInput.get_value() / input;
228 /* --------------------------------------------------------------------------------- */
229 /* --------------------------------------------------------------------------------- */
231 DerivativeFilterImplementation::DerivativeFilterImplementation() :
236 void DerivativeFilterImplementation::initialize( double initvalue )
238 _input_1 = initvalue;
241 //------------------------------------------------------------------------------
242 bool DerivativeFilterImplementation::configure( SGPropertyNode& cfg_node,
243 const std::string& cfg_name,
244 SGPropertyNode& prop_root )
246 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
249 if (cfg_name == "filter-time" ) {
250 _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
257 double DerivativeFilterImplementation::compute( double dt, double input )
259 double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
265 /* --------------------------------------------------------------------------------- */
266 /* --------------------------------------------------------------------------------- */
268 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
273 void MovingAverageFilterImplementation::initialize( double initvalue )
275 _output_1 = initvalue;
278 double MovingAverageFilterImplementation::compute( double dt, double input )
280 typedef std::deque<double>::size_type size_type;
281 size_type samples = _samplesInput.get_value();
283 if (_inputQueue.size() != samples) {
284 // For constant size filters, this code executed once.
285 bool shrunk = _inputQueue.size() > samples;
286 _inputQueue.resize(samples, _output_1);
289 for (size_type ii = 0; ii < samples; ii++)
290 _output_1 += _inputQueue[ii];
291 _output_1 /= samples;
295 double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
297 _output_1 = output_0;
298 _inputQueue.pop_back();
299 _inputQueue.push_front(input);
303 bool MovingAverageFilterImplementation::configure( SGPropertyNode& cfg_node,
304 const std::string& cfg_name,
305 SGPropertyNode& prop_root )
307 if (cfg_name == "samples" ) {
308 _samplesInput.push_back( new InputValue(prop_root, cfg_node, 1) );
315 /* --------------------------------------------------------------------------------- */
316 /* --------------------------------------------------------------------------------- */
318 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
323 void NoiseSpikeFilterImplementation::initialize( double initvalue )
325 _output_1 = initvalue;
328 double NoiseSpikeFilterImplementation::compute( double dt, double input )
330 double delta = input - _output_1;
331 if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
333 double maxChange = _rateOfChangeInput.get_value() * dt;
334 const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
335 if( periodical ) delta = periodical->normalizeSymmetric( delta );
337 if( fabs(delta) <= maxChange )
338 return (_output_1 = input);
340 return (_output_1 = _output_1 + copysign( maxChange, delta ));
343 //------------------------------------------------------------------------------
344 bool NoiseSpikeFilterImplementation::configure( SGPropertyNode& cfg_node,
345 const std::string& cfg_name,
346 SGPropertyNode& prop_root )
348 if (cfg_name == "max-rate-of-change" ) {
349 _rateOfChangeInput.push_back( new InputValue(prop_root, cfg_node, 1) );
356 /* --------------------------------------------------------------------------------- */
358 RateLimitFilterImplementation::RateLimitFilterImplementation() :
363 void RateLimitFilterImplementation::initialize( double initvalue )
365 _output_1 = initvalue;
368 double RateLimitFilterImplementation::compute( double dt, double input )
370 double delta = input - _output_1;
373 if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
375 double maxChange = _rateOfChangeMax.get_value() * dt;
376 double minChange = _rateOfChangeMin.get_value() * dt;
377 // const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
378 // if( periodical ) delta = periodical->normalizeSymmetric( delta );
381 if(delta >= maxChange ) output = _output_1 + maxChange;
382 if(delta <= minChange ) output = _output_1 + minChange;
388 bool RateLimitFilterImplementation::configure( SGPropertyNode& cfg_node,
389 const std::string& cfg_name,
390 SGPropertyNode& prop_root )
392 std::cout << "RateLimitFilterImplementation " << cfg_name << std::endl;
393 if (cfg_name == "max-rate-of-change" ) {
394 _rateOfChangeMax.push_back( new InputValue(prop_root, cfg_node, 1) );
397 if (cfg_name == "min-rate-of-change" ) {
398 _rateOfChangeMin.push_back( new InputValue(prop_root, cfg_node, 1) );
405 /* --------------------------------------------------------------------------------- */
406 /* --------------------------------------------------------------------------------- */
408 ExponentialFilterImplementation::ExponentialFilterImplementation()
409 : _isSecondOrder(false),
415 void ExponentialFilterImplementation::initialize( double initvalue )
417 _output_1 = _output_2 = initvalue;
420 double ExponentialFilterImplementation::compute( double dt, double input )
422 input = GainFilterImplementation::compute( dt, input );
423 double tf = _TfInput.get_value();
427 // avoid negative filter times
428 // and div by zero if -tf == dt
430 double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
433 output_0 = alpha * alpha * input +
434 2 * (1 - alpha) * _output_1 -
435 (1 - alpha) * (1 - alpha) * _output_2;
437 output_0 = alpha * input + (1 - alpha) * _output_1;
439 _output_2 = _output_1;
440 return (_output_1 = output_0);
443 //------------------------------------------------------------------------------
444 bool ExponentialFilterImplementation::configure( SGPropertyNode& cfg_node,
445 const std::string& cfg_name,
446 SGPropertyNode& prop_root )
448 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
451 if (cfg_name == "filter-time" ) {
452 _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
456 if (cfg_name == "type" ) {
457 std::string type(cfg_node.getStringValue());
458 _isSecondOrder = type == "double-exponential";
464 /* --------------------------------------------------------------------------------- */
466 IntegratorFilterImplementation::IntegratorFilterImplementation() :
472 void IntegratorFilterImplementation::initialize( double initvalue )
474 _input_1 = _output_1 = initvalue;
477 //------------------------------------------------------------------------------
478 bool IntegratorFilterImplementation::configure( SGPropertyNode& cfg_node,
479 const std::string& cfg_name,
480 SGPropertyNode& prop_root )
482 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
485 if (cfg_name == "u_min" ) {
486 _minInput.push_back( new InputValue(prop_root, cfg_node, 1) );
489 if (cfg_name == "u_max" ) {
490 _maxInput.push_back( new InputValue(prop_root, cfg_node, 1) );
496 double IntegratorFilterImplementation::compute( double dt, double input )
498 double output = _output_1 + input * _gainInput.get_value() * dt;
499 double u_min = _minInput.get_value();
500 double u_max = _maxInput.get_value();
501 if (output >= u_max) output = u_max; // clamping inside "::compute" prevents integrator wind-up
502 if (output <= u_min) output = u_min;
509 /* --------------------------------------------------------------------------------- */
511 HighPassFilterImplementation::HighPassFilterImplementation() :
518 void HighPassFilterImplementation::initialize( double initvalue )
520 _input_1 = initvalue;
521 _output_1 = initvalue;
524 double HighPassFilterImplementation::compute( double dt, double input )
526 input = GainFilterImplementation::compute( dt, input );
527 double tf = _TfInput.get_value();
531 // avoid negative filter times
532 // and div by zero if -tf == dt
534 double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
535 output = (1 - alpha) * (input - _input_1 + _output_1);
541 //------------------------------------------------------------------------------
542 bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
543 const std::string& cfg_name,
544 SGPropertyNode& prop_root )
546 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
549 if (cfg_name == "filter-time" ) {
550 _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
557 /* --------------------------------------------------------------------------------- */
559 LeadLagFilterImplementation::LeadLagFilterImplementation() :
566 void LeadLagFilterImplementation::initialize( double initvalue )
568 _input_1 = initvalue;
569 _output_1 = initvalue;
572 double LeadLagFilterImplementation::compute( double dt, double input )
574 input = GainFilterImplementation::compute( dt, input );
575 double tfa = _TfaInput.get_value();
576 double tfb = _TfbInput.get_value();
580 // avoid negative filter times
581 // and div by zero if -tf == dt
583 double alpha = tfa > 0.0 ? 1 / ((tfa/dt) + 1) : 1.0;
584 double beta = tfb > 0.0 ? 1 / ((tfb/dt) + 1) : 1.0;
585 output = (1 - beta) * (input / (1 - alpha) - _input_1 + _output_1);
591 //------------------------------------------------------------------------------
592 bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node,
593 const std::string& cfg_name,
594 SGPropertyNode& prop_root )
596 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
599 if (cfg_name == "filter-time-a" ) {
600 _TfaInput.push_back( new InputValue(prop_root, cfg_node, 1) );
603 if (cfg_name == "filter-time-b" ) {
604 _TfbInput.push_back( new InputValue(prop_root, cfg_node, 1) );
609 /* -------------------------------------------------------------------------- */
610 /* Digital Filter Component Implementation */
611 /* -------------------------------------------------------------------------- */
613 DigitalFilter::DigitalFilter() :
615 _initializeTo(INITIALIZE_INPUT)
619 DigitalFilter::~DigitalFilter()
623 //------------------------------------------------------------------------------
624 template<class DigitalFilterType>
625 DigitalFilterImplementation* digitalFilterFactory()
627 return new DigitalFilterType();
630 typedef std::map<std::string, DigitalFilterImplementation*(*)()>
632 static DigitalFilterMap componentForge;
634 //------------------------------------------------------------------------------
635 bool DigitalFilter::configure( SGPropertyNode& prop_root,
636 SGPropertyNode& cfg )
638 if( componentForge.empty() )
640 componentForge["gain" ] = digitalFilterFactory<GainFilterImplementation>;
641 componentForge["exponential" ] = digitalFilterFactory<ExponentialFilterImplementation>;
642 componentForge["double-exponential" ] = digitalFilterFactory<ExponentialFilterImplementation>;
643 componentForge["moving-average" ] = digitalFilterFactory<MovingAverageFilterImplementation>;
644 componentForge["noise-spike" ] = digitalFilterFactory<NoiseSpikeFilterImplementation>;
645 componentForge["rate-limit" ] = digitalFilterFactory<RateLimitFilterImplementation>;
646 componentForge["reciprocal" ] = digitalFilterFactory<ReciprocalFilterImplementation>;
647 componentForge["derivative" ] = digitalFilterFactory<DerivativeFilterImplementation>;
648 componentForge["high-pass" ] = digitalFilterFactory<HighPassFilterImplementation>;
649 componentForge["lead-lag" ] = digitalFilterFactory<LeadLagFilterImplementation>;
650 componentForge["integrator" ] = digitalFilterFactory<IntegratorFilterImplementation>;
653 const std::string type = cfg.getStringValue("type");
654 DigitalFilterMap::iterator component_factory = componentForge.find(type);
655 if( component_factory == componentForge.end() )
657 SG_LOG(SG_AUTOPILOT, SG_WARN, "unhandled filter type '" << type << "'");
661 _implementation = (*component_factory->second)();
662 _implementation->setDigitalFilter( this );
664 for( int i = 0; i < cfg.nChildren(); ++i )
666 SGPropertyNode_ptr child = cfg.getChild(i);
667 std::string cname(child->getName());
669 if( !_implementation->configure(*child, cname, prop_root)
670 && !configure(*child, cname, prop_root)
672 && cname != "params" ) // 'params' is usually used to specify parameters
673 // in PropertList files.
678 "DigitalFilter: unknown config node: " << cname
685 //------------------------------------------------------------------------------
686 bool DigitalFilter::configure( SGPropertyNode& cfg_node,
687 const std::string& cfg_name,
688 SGPropertyNode& prop_root )
690 if( cfg_name == "initialize-to" )
692 std::string s( cfg_node.getStringValue() );
694 _initializeTo = INITIALIZE_INPUT;
695 else if( s == "output" )
696 _initializeTo = INITIALIZE_OUTPUT;
697 else if( s == "none" )
698 _initializeTo = INITIALIZE_NONE;
703 SG_WARN, "DigitalFilter: initialize-to (" << s << ") ignored"
709 return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
712 //------------------------------------------------------------------------------
713 void DigitalFilter::update( bool firstTime, double dt)
715 if( _implementation == NULL ) return;
718 switch( _initializeTo ) {
720 case INITIALIZE_INPUT:
721 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
722 _implementation->initialize( _valueInput.get_value() );
725 case INITIALIZE_OUTPUT:
726 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
727 _implementation->initialize( get_output_value() );
731 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
736 double input = _valueInput.get_value() - _referenceInput.get_value();
737 double output = _implementation->compute( dt, input );
739 set_output_value( output );
742 std::cout << "input:" << input
743 << "\toutput:" << output << std::endl;