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 // integrates x" + ax' + bx + c = 0
158 class DampedOscillationFilterImplementation : public GainFilterImplementation {
160 InputValueList _aInput;
161 InputValueList _bInput;
162 InputValueList _cInput;
166 bool configure( SGPropertyNode& cfg_node,
167 const std::string& cfg_name,
168 SGPropertyNode& prop_root );
170 DampedOscillationFilterImplementation();
171 double compute( double dt, double input );
172 virtual void initialize( double initvalue );
175 class HighPassFilterImplementation : public GainFilterImplementation {
177 InputValueList _TfInput;
180 bool configure( SGPropertyNode& cfg_node,
181 const std::string& cfg_name,
182 SGPropertyNode& prop_root );
184 HighPassFilterImplementation();
185 double compute( double dt, double input );
186 virtual void initialize( double initvalue );
188 class LeadLagFilterImplementation : public GainFilterImplementation {
190 InputValueList _TfaInput;
191 InputValueList _TfbInput;
194 bool configure( SGPropertyNode& cfg_node,
195 const std::string& cfg_name,
196 SGPropertyNode& prop_root );
198 LeadLagFilterImplementation();
199 double compute( double dt, double input );
200 virtual void initialize( double initvalue );
202 /* --------------------------------------------------------------------------------- */
203 /* --------------------------------------------------------------------------------- */
205 } // namespace FGXMLAutopilot
207 using namespace FGXMLAutopilot;
209 //------------------------------------------------------------------------------
210 DigitalFilterImplementation::DigitalFilterImplementation() :
216 //------------------------------------------------------------------------------
217 double GainFilterImplementation::compute( double dt, double input )
219 return _gainInput.get_value() * input;
222 bool GainFilterImplementation::configure( SGPropertyNode& cfg_node,
223 const std::string& cfg_name,
224 SGPropertyNode& prop_root )
226 if (cfg_name == "gain" ) {
227 _gainInput.push_back( new InputValue(prop_root, cfg_node, 1) );
234 /* --------------------------------------------------------------------------------- */
235 /* --------------------------------------------------------------------------------- */
237 double ReciprocalFilterImplementation::compute( double dt, double input )
239 if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
240 return SGLimitsd::max();
242 return _gainInput.get_value() / input;
246 /* --------------------------------------------------------------------------------- */
247 /* --------------------------------------------------------------------------------- */
249 DerivativeFilterImplementation::DerivativeFilterImplementation() :
254 void DerivativeFilterImplementation::initialize( double initvalue )
256 _input_1 = initvalue;
259 //------------------------------------------------------------------------------
260 bool DerivativeFilterImplementation::configure( SGPropertyNode& cfg_node,
261 const std::string& cfg_name,
262 SGPropertyNode& prop_root )
264 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
267 if (cfg_name == "filter-time" ) {
268 _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
275 double DerivativeFilterImplementation::compute( double dt, double input )
277 double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
283 /* --------------------------------------------------------------------------------- */
284 /* --------------------------------------------------------------------------------- */
286 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
291 void MovingAverageFilterImplementation::initialize( double initvalue )
293 _output_1 = initvalue;
296 double MovingAverageFilterImplementation::compute( double dt, double input )
298 typedef std::deque<double>::size_type size_type;
299 size_type samples = _samplesInput.get_value();
301 if (_inputQueue.size() != samples) {
302 // For constant size filters, this code executed once.
303 bool shrunk = _inputQueue.size() > samples;
304 _inputQueue.resize(samples, _output_1);
307 for (size_type ii = 0; ii < samples; ii++)
308 _output_1 += _inputQueue[ii];
309 _output_1 /= samples;
313 double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
315 _output_1 = output_0;
316 _inputQueue.pop_back();
317 _inputQueue.push_front(input);
321 bool MovingAverageFilterImplementation::configure( SGPropertyNode& cfg_node,
322 const std::string& cfg_name,
323 SGPropertyNode& prop_root )
325 if (cfg_name == "samples" ) {
326 _samplesInput.push_back( new InputValue(prop_root, cfg_node, 1) );
333 /* --------------------------------------------------------------------------------- */
334 /* --------------------------------------------------------------------------------- */
336 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
341 void NoiseSpikeFilterImplementation::initialize( double initvalue )
343 _output_1 = initvalue;
346 double NoiseSpikeFilterImplementation::compute( double dt, double input )
348 double delta = input - _output_1;
349 if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
351 double maxChange = _rateOfChangeInput.get_value() * dt;
352 const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
353 if( periodical ) delta = periodical->normalizeSymmetric( delta );
355 if( fabs(delta) <= maxChange )
356 return (_output_1 = input);
358 return (_output_1 = _output_1 + copysign( maxChange, delta ));
361 //------------------------------------------------------------------------------
362 bool NoiseSpikeFilterImplementation::configure( SGPropertyNode& cfg_node,
363 const std::string& cfg_name,
364 SGPropertyNode& prop_root )
366 if (cfg_name == "max-rate-of-change" ) {
367 _rateOfChangeInput.push_back( new InputValue(prop_root, cfg_node, 1) );
374 /* --------------------------------------------------------------------------------- */
376 RateLimitFilterImplementation::RateLimitFilterImplementation() :
381 void RateLimitFilterImplementation::initialize( double initvalue )
383 _output_1 = initvalue;
386 double RateLimitFilterImplementation::compute( double dt, double input )
388 double delta = input - _output_1;
391 if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
393 double maxChange = _rateOfChangeMax.get_value() * dt;
394 double minChange = _rateOfChangeMin.get_value() * dt;
395 // const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
396 // if( periodical ) delta = periodical->normalizeSymmetric( delta );
399 if(delta >= maxChange ) output = _output_1 + maxChange;
400 if(delta <= minChange ) output = _output_1 + minChange;
406 bool RateLimitFilterImplementation::configure( SGPropertyNode& cfg_node,
407 const std::string& cfg_name,
408 SGPropertyNode& prop_root )
410 // std::cout << "RateLimitFilterImplementation " << cfg_name << std::endl;
411 if (cfg_name == "max-rate-of-change" ) {
412 _rateOfChangeMax.push_back( new InputValue(prop_root, cfg_node, 1) );
415 if (cfg_name == "min-rate-of-change" ) {
416 _rateOfChangeMin.push_back( new InputValue(prop_root, cfg_node, 1) );
423 /* --------------------------------------------------------------------------------- */
424 /* --------------------------------------------------------------------------------- */
426 ExponentialFilterImplementation::ExponentialFilterImplementation()
427 : _isSecondOrder(false),
433 void ExponentialFilterImplementation::initialize( double initvalue )
435 _output_1 = _output_2 = initvalue;
438 double ExponentialFilterImplementation::compute( double dt, double input )
440 input = GainFilterImplementation::compute( dt, input );
441 double tf = _TfInput.get_value();
445 // avoid negative filter times
446 // and div by zero if -tf == dt
448 double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
451 output_0 = alpha * alpha * input +
452 2 * (1 - alpha) * _output_1 -
453 (1 - alpha) * (1 - alpha) * _output_2;
455 output_0 = alpha * input + (1 - alpha) * _output_1;
457 _output_2 = _output_1;
458 return (_output_1 = output_0);
461 //------------------------------------------------------------------------------
462 bool ExponentialFilterImplementation::configure( SGPropertyNode& cfg_node,
463 const std::string& cfg_name,
464 SGPropertyNode& prop_root )
466 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
469 if (cfg_name == "filter-time" ) {
470 _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
474 if (cfg_name == "type" ) {
475 std::string type(cfg_node.getStringValue());
476 _isSecondOrder = type == "double-exponential";
482 /* --------------------------------------------------------------------------------- */
484 IntegratorFilterImplementation::IntegratorFilterImplementation() :
490 void IntegratorFilterImplementation::initialize( double initvalue )
492 _input_1 = _output_1 = initvalue;
495 //------------------------------------------------------------------------------
496 bool IntegratorFilterImplementation::configure( SGPropertyNode& cfg_node,
497 const std::string& cfg_name,
498 SGPropertyNode& prop_root )
500 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
503 if (cfg_name == "u_min" ) {
504 _minInput.push_back( new InputValue(prop_root, cfg_node, 1) );
507 if (cfg_name == "u_max" ) {
508 _maxInput.push_back( new InputValue(prop_root, cfg_node, 1) );
514 double IntegratorFilterImplementation::compute( double dt, double input )
516 double output = _output_1 + input * _gainInput.get_value() * dt;
517 double u_min = _minInput.get_value();
518 double u_max = _maxInput.get_value();
519 if (output >= u_max) output = u_max; // clamping inside "::compute" prevents integrator wind-up
520 if (output <= u_min) output = u_min;
527 /* --------------------------------------------------------------------------------- */
528 DampedOscillationFilterImplementation::DampedOscillationFilterImplementation() :
533 void DampedOscillationFilterImplementation::initialize( double initvalue )
535 _x2 = _x1 = _x0 = initvalue;
538 bool DampedOscillationFilterImplementation::configure( SGPropertyNode& cfg_node,
539 const std::string& cfg_name,
540 SGPropertyNode& prop_root )
542 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
545 if (cfg_name == "a" ) {
546 _aInput.push_back( new InputValue(prop_root, cfg_node, 1) );
549 if (cfg_name == "b" ) {
550 _bInput.push_back( new InputValue(prop_root, cfg_node, 1) );
553 if (cfg_name == "c" ) {
554 _cInput.push_back( new InputValue(prop_root, cfg_node, 1) );
560 double DampedOscillationFilterImplementation::compute( double dt, double input )
562 if (fabs(input) > 1e-15) {
563 double dz = dt * input;
567 double a = _aInput.get_value();
568 double b = _bInput.get_value();
569 double c = _cInput.get_value();
570 _x0 = (_x1 * (2. + dt * (a - b * dt)) - _x2 - c * dt * dt) / (1. + a * dt);
577 /* --------------------------------------------------------------------------------- */
579 HighPassFilterImplementation::HighPassFilterImplementation() :
586 void HighPassFilterImplementation::initialize( double initvalue )
588 _input_1 = initvalue;
589 _output_1 = initvalue;
592 double HighPassFilterImplementation::compute( double dt, double input )
594 input = GainFilterImplementation::compute( dt, input );
595 double tf = _TfInput.get_value();
599 // avoid negative filter times
600 // and div by zero if -tf == dt
602 double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
603 output = (1 - alpha) * (input - _input_1 + _output_1);
609 //------------------------------------------------------------------------------
610 bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
611 const std::string& cfg_name,
612 SGPropertyNode& prop_root )
614 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
617 if (cfg_name == "filter-time" ) {
618 _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
625 /* --------------------------------------------------------------------------------- */
627 LeadLagFilterImplementation::LeadLagFilterImplementation() :
634 void LeadLagFilterImplementation::initialize( double initvalue )
636 _input_1 = initvalue;
637 _output_1 = initvalue;
640 double LeadLagFilterImplementation::compute( double dt, double input )
642 input = GainFilterImplementation::compute( dt, input );
643 double tfa = _TfaInput.get_value();
644 double tfb = _TfbInput.get_value();
648 // avoid negative filter times
649 // and div by zero if -tf == dt
651 double alpha = tfa > 0.0 ? 1 / ((tfa/dt) + 1) : 1.0;
652 double beta = tfb > 0.0 ? 1 / ((tfb/dt) + 1) : 1.0;
653 output = (1 - beta) * (input / (1 - alpha) - _input_1 + _output_1);
659 //------------------------------------------------------------------------------
660 bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node,
661 const std::string& cfg_name,
662 SGPropertyNode& prop_root )
664 if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
667 if (cfg_name == "filter-time-a" ) {
668 _TfaInput.push_back( new InputValue(prop_root, cfg_node, 1) );
671 if (cfg_name == "filter-time-b" ) {
672 _TfbInput.push_back( new InputValue(prop_root, cfg_node, 1) );
677 /* -------------------------------------------------------------------------- */
678 /* Digital Filter Component Implementation */
679 /* -------------------------------------------------------------------------- */
681 DigitalFilter::DigitalFilter() :
683 _initializeTo(INITIALIZE_INPUT)
687 DigitalFilter::~DigitalFilter()
691 //------------------------------------------------------------------------------
692 template<class DigitalFilterType>
693 DigitalFilterImplementation* digitalFilterFactory()
695 return new DigitalFilterType();
698 typedef std::map<std::string, DigitalFilterImplementation*(*)()>
700 static DigitalFilterMap componentForge;
702 //------------------------------------------------------------------------------
703 bool DigitalFilter::configure( SGPropertyNode& prop_root,
704 SGPropertyNode& cfg )
706 if( componentForge.empty() )
708 componentForge["gain" ] = digitalFilterFactory<GainFilterImplementation>;
709 componentForge["exponential" ] = digitalFilterFactory<ExponentialFilterImplementation>;
710 componentForge["double-exponential" ] = digitalFilterFactory<ExponentialFilterImplementation>;
711 componentForge["moving-average" ] = digitalFilterFactory<MovingAverageFilterImplementation>;
712 componentForge["noise-spike" ] = digitalFilterFactory<NoiseSpikeFilterImplementation>;
713 componentForge["rate-limit" ] = digitalFilterFactory<RateLimitFilterImplementation>;
714 componentForge["reciprocal" ] = digitalFilterFactory<ReciprocalFilterImplementation>;
715 componentForge["derivative" ] = digitalFilterFactory<DerivativeFilterImplementation>;
716 componentForge["high-pass" ] = digitalFilterFactory<HighPassFilterImplementation>;
717 componentForge["lead-lag" ] = digitalFilterFactory<LeadLagFilterImplementation>;
718 componentForge["integrator" ] = digitalFilterFactory<IntegratorFilterImplementation>;
719 componentForge["damped-oscillation" ] = digitalFilterFactory<DampedOscillationFilterImplementation>;
722 const std::string type = cfg.getStringValue("type");
723 DigitalFilterMap::iterator component_factory = componentForge.find(type);
724 if( component_factory == componentForge.end() )
726 SG_LOG(SG_AUTOPILOT, SG_WARN, "unhandled filter type '" << type << "'");
730 _implementation = (*component_factory->second)();
731 _implementation->setDigitalFilter( this );
733 for( int i = 0; i < cfg.nChildren(); ++i )
735 SGPropertyNode_ptr child = cfg.getChild(i);
736 std::string cname(child->getName());
738 if( !_implementation->configure(*child, cname, prop_root)
739 && !configure(*child, cname, prop_root)
741 && cname != "params" ) // 'params' is usually used to specify parameters
742 // in PropertList files.
747 "DigitalFilter: unknown config node: " << cname
754 //------------------------------------------------------------------------------
755 bool DigitalFilter::configure( SGPropertyNode& cfg_node,
756 const std::string& cfg_name,
757 SGPropertyNode& prop_root )
759 if( cfg_name == "initialize-to" )
761 std::string s( cfg_node.getStringValue() );
763 _initializeTo = INITIALIZE_INPUT;
764 else if( s == "output" )
765 _initializeTo = INITIALIZE_OUTPUT;
766 else if( s == "none" )
767 _initializeTo = INITIALIZE_NONE;
772 SG_WARN, "DigitalFilter: initialize-to (" << s << ") ignored"
778 return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
781 //------------------------------------------------------------------------------
782 void DigitalFilter::update( bool firstTime, double dt)
784 if( _implementation == NULL ) return;
787 switch( _initializeTo ) {
789 case INITIALIZE_INPUT:
790 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
791 _implementation->initialize( _valueInput.get_value() );
794 case INITIALIZE_OUTPUT:
795 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
796 _implementation->initialize( get_output_value() );
800 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
805 double input = _valueInput.get_value() - _referenceInput.get_value();
806 double output = _implementation->compute( dt, input );
808 set_output_value( output );
811 std::cout << "input:" << input
812 << "\toutput:" << output << std::endl;