src/Autopilot/digitalfilter.cxx

   1 // digitalfilter.cxx - a selection of digital filters
   2 //
   3 // Written by Torsten Dreyer
   4 // Based heavily on work created by Curtis Olson, started January 2004.
   5 //
   6 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   7 // Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
   8 //
   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
  12 //
  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.
  17 //
  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.
  22 //
  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.
  26 //
  27
  28 #include "digitalfilter.hxx"
  29 #include <deque>
  30
  31 namespace FGXMLAutopilot
  32 {
  33
  34 /**
  35  *
  36  *
  37  */
  38 class DigitalFilterImplementation:
  39   public SGReferenced
  40 {
  41   public:
  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;
  49
  50     void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
  51
  52   protected:
  53     DigitalFilter * _digitalFilter;
  54 };
  55
  56 /* --------------------------------------------------------------------------------- */
  57 /* --------------------------------------------------------------------------------- */
  58 class GainFilterImplementation : public DigitalFilterImplementation {
  59 protected:
  60   InputValueList _gainInput;
  61   bool configure( SGPropertyNode& cfg_node,
  62                   const std::string& cfg_name,
  63                   SGPropertyNode& prop_root );
  64 public:
  65   GainFilterImplementation() : _gainInput(1.0) {}
  66   double compute(  double dt, double input );
  67 };
  68
  69 class ReciprocalFilterImplementation : public GainFilterImplementation {
  70 public:
  71   double compute(  double dt, double input );
  72 };
  73
  74 class DerivativeFilterImplementation : public GainFilterImplementation {
  75   InputValueList _TfInput;
  76   double _input_1;
  77   bool configure( SGPropertyNode& cfg_node,
  78                   const std::string& cfg_name,
  79                   SGPropertyNode& prop_root );
  80 public:
  81   DerivativeFilterImplementation();
  82   double compute(  double dt, double input );
  83   virtual void initialize( double initvalue );
  84 };
  85
  86 class ExponentialFilterImplementation : public GainFilterImplementation {
  87 protected:
  88   InputValueList _TfInput;
  89   bool configure( SGPropertyNode& cfg_node,
  90                   const std::string& cfg_name,
  91                   SGPropertyNode& prop_root );
  92   bool _isSecondOrder;
  93   double _output_1, _output_2;
  94 public:
  95   ExponentialFilterImplementation();
  96   double compute(  double dt, double input );
  97   virtual void initialize( double initvalue );
  98 };
  99
 100 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
 101 protected:
 102   InputValueList _samplesInput;
 103   double _output_1;
 104   std::deque <double> _inputQueue;
 105   bool configure( SGPropertyNode& cfg_node,
 106                   const std::string& cfg_name,
 107                   SGPropertyNode& prop_root );
 108 public:
 109   MovingAverageFilterImplementation();
 110   double compute(  double dt, double input );
 111   virtual void initialize( double initvalue );
 112 };
 113
 114 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
 115 protected:
 116   double _output_1;
 117   InputValueList _rateOfChangeInput;
 118   bool configure( SGPropertyNode& cfg_node,
 119                   const std::string& cfg_name,
 120                   SGPropertyNode& prop_root );
 121 public:
 122   NoiseSpikeFilterImplementation();
 123   double compute(  double dt, double input );
 124   virtual void initialize( double initvalue );
 125 };
 126
 127 class RateLimitFilterImplementation : public DigitalFilterImplementation {
 128 protected:
 129   double _output_1;
 130   InputValueList _rateOfChangeMax;
 131   InputValueList _rateOfChangeMin ;
 132   bool configure( SGPropertyNode& cfg_node,
 133                   const std::string& cfg_name,
 134                   SGPropertyNode& prop_root );
 135 public:
 136   RateLimitFilterImplementation();
 137   double compute(  double dt, double input );
 138   virtual void initialize( double initvalue );
 139 };
 140
 141 class IntegratorFilterImplementation : public GainFilterImplementation {
 142 protected:
 143   InputValueList _TfInput;
 144   InputValueList _minInput;
 145   InputValueList _maxInput;
 146   double _input_1;
 147   double _output_1;
 148   bool configure( SGPropertyNode& cfg_node,
 149                   const std::string& cfg_name,
 150                   SGPropertyNode& prop_root );
 151 public:
 152   IntegratorFilterImplementation();
 153   double compute(  double dt, double input );
 154   virtual void initialize( double initvalue );
 155 };
 156
 157 // integrates x" + ax' + bx + c = 0
 158 class DampedOscillationFilterImplementation : public GainFilterImplementation {
 159 protected:
 160   InputValueList _aInput;
 161   InputValueList _bInput;
 162   InputValueList _cInput;
 163   double _x2;
 164   double _x1;
 165   double _x0;
 166   bool configure( SGPropertyNode& cfg_node,
 167                   const std::string& cfg_name,
 168                   SGPropertyNode& prop_root );
 169 public:
 170   DampedOscillationFilterImplementation();
 171   double compute(  double dt, double input );
 172   virtual void initialize( double initvalue );
 173 };
 174
 175 class HighPassFilterImplementation : public GainFilterImplementation {
 176 protected:
 177   InputValueList _TfInput;
 178   double _input_1;
 179   double _output_1;
 180   bool configure( SGPropertyNode& cfg_node,
 181                   const std::string& cfg_name,
 182                   SGPropertyNode& prop_root );
 183 public:
 184   HighPassFilterImplementation();
 185   double compute(  double dt, double input );
 186   virtual void initialize( double initvalue );
 187 };
 188 class LeadLagFilterImplementation : public GainFilterImplementation {
 189 protected:
 190   InputValueList _TfaInput;
 191   InputValueList _TfbInput;
 192   double _input_1;
 193   double _output_1;
 194   bool configure( SGPropertyNode& cfg_node,
 195                   const std::string& cfg_name,
 196                   SGPropertyNode& prop_root );
 197 public:
 198   LeadLagFilterImplementation();
 199   double compute(  double dt, double input );
 200   virtual void initialize( double initvalue );
 201 };
 202 /* --------------------------------------------------------------------------------- */
 203 /* --------------------------------------------------------------------------------- */
 204
 205 } // namespace FGXMLAutopilot
 206
 207 using namespace FGXMLAutopilot;
 208
 209 //------------------------------------------------------------------------------
 210 DigitalFilterImplementation::DigitalFilterImplementation() :
 211   _digitalFilter(NULL)
 212 {
 213
 214 }
 215
 216 //------------------------------------------------------------------------------
 217 double GainFilterImplementation::compute(  double dt, double input )
 218 {
 219   return _gainInput.get_value() * input;
 220 }
 221
 222 bool GainFilterImplementation::configure( SGPropertyNode& cfg_node,
 223                                           const std::string& cfg_name,
 224                                           SGPropertyNode& prop_root )
 225 {
 226   if (cfg_name == "gain" ) {
 227     _gainInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 228     return true;
 229   }
 230
 231   return false;
 232 }
 233
 234 /* --------------------------------------------------------------------------------- */
 235 /* --------------------------------------------------------------------------------- */
 236
 237 double ReciprocalFilterImplementation::compute(  double dt, double input )
 238 {
 239   if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
 240     return SGLimitsd::max();
 241
 242   return _gainInput.get_value() / input;
 243
 244 }
 245
 246 /* --------------------------------------------------------------------------------- */
 247 /* --------------------------------------------------------------------------------- */
 248
 249 DerivativeFilterImplementation::DerivativeFilterImplementation() :
 250   _input_1(0.0)
 251 {
 252 }
 253
 254 void DerivativeFilterImplementation::initialize( double initvalue )
 255 {
 256   _input_1 = initvalue;
 257 }
 258
 259 //------------------------------------------------------------------------------
 260 bool DerivativeFilterImplementation::configure( SGPropertyNode& cfg_node,
 261                                                 const std::string& cfg_name,
 262                                                 SGPropertyNode& prop_root )
 263 {
 264   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 265     return true;
 266
 267   if (cfg_name == "filter-time" ) {
 268     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 269     return true;
 270   }
 271
 272   return false;
 273 }
 274
 275 double DerivativeFilterImplementation::compute(  double dt, double input )
 276 {
 277   double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
 278   _input_1 = input;
 279   return output;
 280
 281 }
 282
 283 /* --------------------------------------------------------------------------------- */
 284 /* --------------------------------------------------------------------------------- */
 285
 286 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
 287   _output_1(0.0)
 288 {
 289 }
 290
 291 void MovingAverageFilterImplementation::initialize( double initvalue )
 292 {
 293   _output_1 = initvalue;
 294 }
 295
 296 double MovingAverageFilterImplementation::compute(  double dt, double input )
 297 {
 298   typedef std::deque<double>::size_type size_type;
 299   size_type samples = _samplesInput.get_value();
 300
 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);
 305     if (shrunk) {
 306       _output_1 = 0.0;
 307       for (size_type ii = 0; ii < samples; ii++)
 308       _output_1 += _inputQueue[ii];
 309       _output_1 /= samples;
 310     }
 311   }
 312
 313   double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
 314
 315   _output_1 = output_0;
 316   _inputQueue.pop_back();
 317   _inputQueue.push_front(input);
 318   return output_0;
 319 }
 320
 321 bool MovingAverageFilterImplementation::configure( SGPropertyNode& cfg_node,
 322                                                    const std::string& cfg_name,
 323                                                    SGPropertyNode& prop_root )
 324 {
 325   if (cfg_name == "samples" ) {
 326     _samplesInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 327     return true;
 328   }
 329
 330   return false;
 331 }
 332
 333 /* --------------------------------------------------------------------------------- */
 334 /* --------------------------------------------------------------------------------- */
 335
 336 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
 337   _output_1(0.0)
 338 {
 339 }
 340
 341 void NoiseSpikeFilterImplementation::initialize( double initvalue )
 342 {
 343   _output_1 = initvalue;
 344 }
 345
 346 double NoiseSpikeFilterImplementation::compute(  double dt, double input )
 347 {
 348   double delta = input - _output_1;
 349   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 350
 351   double maxChange = _rateOfChangeInput.get_value() * dt;
 352   const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 353   if( periodical ) delta = periodical->normalizeSymmetric( delta );
 354
 355   if( fabs(delta) <= maxChange )
 356     return (_output_1 = input);
 357   else
 358     return (_output_1 = _output_1 + copysign( maxChange, delta ));
 359 }
 360
 361 //------------------------------------------------------------------------------
 362 bool NoiseSpikeFilterImplementation::configure( SGPropertyNode& cfg_node,
 363                                                 const std::string& cfg_name,
 364                                                 SGPropertyNode& prop_root )
 365 {
 366   if (cfg_name == "max-rate-of-change" ) {
 367     _rateOfChangeInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 368     return true;
 369   }
 370
 371   return false;
 372 }
 373
 374 /* --------------------------------------------------------------------------------- */
 375
 376 RateLimitFilterImplementation::RateLimitFilterImplementation() :
 377   _output_1(0.0)
 378 {
 379 }
 380
 381 void RateLimitFilterImplementation::initialize( double initvalue )
 382 {
 383   _output_1 = initvalue;
 384 }
 385
 386 double RateLimitFilterImplementation::compute(  double dt, double input )
 387 {
 388   double delta = input - _output_1;
 389   double output;
 390
 391   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 392
 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 );
 397
 398   output = input;
 399   if(delta >= maxChange ) output = _output_1 + maxChange;
 400   if(delta <= minChange ) output = _output_1 + minChange;
 401   _output_1 = output;
 402
 403   return (output);
 404 }
 405
 406 bool RateLimitFilterImplementation::configure( SGPropertyNode& cfg_node,
 407                                                const std::string& cfg_name,
 408                                                SGPropertyNode& prop_root )
 409 {
 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) );
 413     return true;
 414   }
 415   if (cfg_name == "min-rate-of-change" ) {
 416     _rateOfChangeMin.push_back( new InputValue(prop_root, cfg_node, 1) );
 417     return true;
 418   }
 419
 420   return false;
 421 }
 422
 423 /* --------------------------------------------------------------------------------- */
 424 /* --------------------------------------------------------------------------------- */
 425
 426 ExponentialFilterImplementation::ExponentialFilterImplementation()
 427   : _isSecondOrder(false),
 428     _output_1(0.0),
 429     _output_2(0.0)
 430 {
 431 }
 432
 433 void ExponentialFilterImplementation::initialize( double initvalue )
 434 {
 435   _output_1 = _output_2 = initvalue;
 436 }
 437
 438 double ExponentialFilterImplementation::compute(  double dt, double input )
 439 {
 440   input = GainFilterImplementation::compute( dt, input );
 441   double tf = _TfInput.get_value();
 442
 443   double output_0;
 444
 445   // avoid negative filter times
 446   // and div by zero if -tf == dt
 447
 448   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 449
 450   if(_isSecondOrder) {
 451     output_0 = alpha * alpha * input +
 452                2 * (1 - alpha) * _output_1 -
 453               (1 - alpha) * (1 - alpha) * _output_2;
 454   } else {
 455     output_0 = alpha * input + (1 - alpha) * _output_1;
 456   }
 457   _output_2 = _output_1;
 458   return (_output_1 = output_0);
 459 }
 460
 461 //------------------------------------------------------------------------------
 462 bool ExponentialFilterImplementation::configure( SGPropertyNode& cfg_node,
 463                                                  const std::string& cfg_name,
 464                                                  SGPropertyNode& prop_root )
 465 {
 466   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 467     return true;
 468
 469   if (cfg_name == "filter-time" ) {
 470     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 471     return true;
 472   }
 473
 474   if (cfg_name == "type" ) {
 475     std::string type(cfg_node.getStringValue());
 476     _isSecondOrder = type == "double-exponential";
 477   }
 478
 479   return false;
 480 }
 481
 482 /* --------------------------------------------------------------------------------- */
 483
 484 IntegratorFilterImplementation::IntegratorFilterImplementation() :
 485   _input_1(0.0),
 486   _output_1(0.0)
 487 {
 488 }
 489
 490 void IntegratorFilterImplementation::initialize( double initvalue )
 491 {
 492   _input_1 = _output_1 = initvalue;
 493 }
 494
 495 //------------------------------------------------------------------------------
 496 bool IntegratorFilterImplementation::configure( SGPropertyNode& cfg_node,
 497                                                 const std::string& cfg_name,
 498                                                 SGPropertyNode& prop_root )
 499 {
 500   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 501     return true;
 502
 503   if (cfg_name == "u_min" ) {
 504     _minInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 505     return true;
 506   }
 507   if (cfg_name == "u_max" ) {
 508     _maxInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 509     return true;
 510   }
 511   return false;
 512 }
 513
 514 double IntegratorFilterImplementation::compute(  double dt, double input )
 515 {
 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;
 521   _input_1 = input;
 522   _output_1 = output;
 523   return output;
 524
 525 }
 526
 527 /* --------------------------------------------------------------------------------- */
 528 DampedOscillationFilterImplementation::DampedOscillationFilterImplementation() :
 529   _x0(0.0)
 530 {
 531 }
 532
 533 void DampedOscillationFilterImplementation::initialize( double initvalue )
 534 {
 535   _x2 = _x1 = _x0 = initvalue;
 536 }
 537
 538 bool DampedOscillationFilterImplementation::configure( SGPropertyNode& cfg_node,
 539                                                 const std::string& cfg_name,
 540                                                 SGPropertyNode& prop_root )
 541 {
 542   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 543     return true;
 544
 545   if (cfg_name == "a" ) {
 546     _aInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 547     return true;
 548   }
 549   if (cfg_name == "b" ) {
 550     _bInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 551     return true;
 552   }
 553   if (cfg_name == "c" ) {
 554     _cInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 555     return true;
 556   }
 557   return false;
 558 }
 559
 560 double DampedOscillationFilterImplementation::compute( double dt, double input )
 561 {
 562   if (fabs(input) > 1e-15) {
 563     double dz = dt * input;
 564     _x0 = _x1 - dz;
 565     _x2 = _x1 + dz;
 566   } else {
 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);
 571     _x2 = _x1;
 572     _x1 = _x0;
 573   }
 574   return _x0;
 575 }
 576
 577 /* --------------------------------------------------------------------------------- */
 578
 579 HighPassFilterImplementation::HighPassFilterImplementation() :
 580   _input_1(0.0),
 581   _output_1(0.0)
 582
 583 {
 584 }
 585
 586 void HighPassFilterImplementation::initialize( double initvalue )
 587 {
 588   _input_1 = initvalue;
 589   _output_1 = initvalue;
 590 }
 591
 592 double HighPassFilterImplementation::compute(  double dt, double input )
 593 {
 594   input = GainFilterImplementation::compute( dt, input );
 595   double tf = _TfInput.get_value();
 596
 597   double output;
 598
 599   // avoid negative filter times
 600   // and div by zero if -tf == dt
 601
 602   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 603   output = (1 - alpha) * (input - _input_1 +  _output_1);
 604   _input_1 = input;
 605   _output_1 = output;
 606   return output;
 607 }
 608
 609 //------------------------------------------------------------------------------
 610 bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
 611                                               const std::string& cfg_name,
 612                                               SGPropertyNode& prop_root )
 613 {
 614   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 615     return true;
 616
 617   if (cfg_name == "filter-time" ) {
 618     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 619     return true;
 620   }
 621
 622   return false;
 623 }
 624
 625 /* --------------------------------------------------------------------------------- */
 626
 627 LeadLagFilterImplementation::LeadLagFilterImplementation() :
 628   _input_1(0.0),
 629   _output_1(0.0)
 630
 631 {
 632 }
 633
 634 void LeadLagFilterImplementation::initialize( double initvalue )
 635 {
 636   _input_1 = initvalue;
 637   _output_1 = initvalue;
 638 }
 639
 640 double LeadLagFilterImplementation::compute(  double dt, double input )
 641 {
 642   input = GainFilterImplementation::compute( dt, input );
 643   double tfa = _TfaInput.get_value();
 644   double tfb = _TfbInput.get_value();
 645
 646   double output;
 647
 648   // avoid negative filter times
 649   // and div by zero if -tf == dt
 650
 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);
 654   _input_1 = input;
 655   _output_1 = output;
 656   return output;
 657 }
 658
 659 //------------------------------------------------------------------------------
 660 bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node,
 661                                              const std::string& cfg_name,
 662                                              SGPropertyNode& prop_root )
 663 {
 664   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 665     return true;
 666
 667   if (cfg_name == "filter-time-a" ) {
 668     _TfaInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 669     return true;
 670   }
 671   if (cfg_name == "filter-time-b" ) {
 672     _TfbInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 673     return true;
 674   }
 675   return false;
 676 }
 677 /* -------------------------------------------------------------------------- */
 678 /* Digital Filter Component Implementation                                    */
 679 /* -------------------------------------------------------------------------- */
 680
 681 DigitalFilter::DigitalFilter() :
 682     AnalogComponent(),
 683     _initializeTo(INITIALIZE_INPUT)
 684 {
 685 }
 686
 687 DigitalFilter::~DigitalFilter()
 688 {
 689 }
 690
 691 //------------------------------------------------------------------------------
 692 template<class DigitalFilterType>
 693 DigitalFilterImplementation* digitalFilterFactory()
 694 {
 695   return new DigitalFilterType();
 696 }
 697
 698 typedef std::map<std::string, DigitalFilterImplementation*(*)()>
 699 DigitalFilterMap;
 700 static DigitalFilterMap componentForge;
 701
 702 //------------------------------------------------------------------------------
 703 bool DigitalFilter::configure( SGPropertyNode& prop_root,
 704                                SGPropertyNode& cfg )
 705 {
 706   if( componentForge.empty() )
 707   {
 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>;
 720   }
 721
 722   const std::string type = cfg.getStringValue("type");
 723   DigitalFilterMap::iterator component_factory = componentForge.find(type);
 724   if( component_factory == componentForge.end() )
 725   {
 726     SG_LOG(SG_AUTOPILOT, SG_WARN, "unhandled filter type '" << type << "'");
 727     return false;
 728   }
 729
 730   _implementation = (*component_factory->second)();
 731   _implementation->setDigitalFilter( this );
 732
 733   for( int i = 0; i < cfg.nChildren(); ++i )
 734   {
 735     SGPropertyNode_ptr child = cfg.getChild(i);
 736     std::string cname(child->getName());
 737
 738     if(    !_implementation->configure(*child, cname, prop_root)
 739         && !configure(*child, cname, prop_root)
 740         && cname != "type"
 741         && cname != "params" ) // 'params' is usually used to specify parameters
 742                                // in PropertList files.
 743       SG_LOG
 744       (
 745         SG_AUTOPILOT,
 746         SG_WARN,
 747         "DigitalFilter: unknown config node: " << cname
 748       );
 749   }
 750
 751   return true;
 752 }
 753
 754 //------------------------------------------------------------------------------
 755 bool DigitalFilter::configure( SGPropertyNode& cfg_node,
 756                                const std::string& cfg_name,
 757                                SGPropertyNode& prop_root )
 758 {
 759   if( cfg_name == "initialize-to" )
 760   {
 761     std::string s( cfg_node.getStringValue() );
 762     if( s == "input" )
 763       _initializeTo = INITIALIZE_INPUT;
 764     else if( s == "output" )
 765       _initializeTo = INITIALIZE_OUTPUT;
 766     else if( s == "none" )
 767       _initializeTo = INITIALIZE_NONE;
 768     else
 769       SG_LOG
 770       (
 771         SG_AUTOPILOT,
 772         SG_WARN, "DigitalFilter: initialize-to (" << s << ") ignored"
 773       );
 774
 775     return true;
 776   }
 777
 778   return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
 779 }
 780
 781 //------------------------------------------------------------------------------
 782 void DigitalFilter::update( bool firstTime, double dt)
 783 {
 784   if( _implementation == NULL ) return;
 785
 786   if( firstTime ) {
 787     switch( _initializeTo ) {
 788
 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() );
 792         break;
 793
 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() );
 797         break;
 798
 799       default:
 800         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
 801         break;
 802     }
 803   }
 804
 805   double input = _valueInput.get_value() - _referenceInput.get_value();
 806   double output = _implementation->compute( dt, input );
 807
 808   set_output_value( output );
 809
 810   if(_debug) {
 811     std::cout << "input:" << input
 812               << "\toutput:" << output << std::endl;
 813   }
 814 }