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 "functor.hxx"
  30 #include <deque>
  31
  32 using std::map;
  33 using std::string;
  34 using std::endl;
  35 using std::cout;
  36
  37 namespace FGXMLAutopilot {
  38
  39 /**
  40  *
  41  *
  42  */
  43 class DigitalFilterImplementation : public SGReferenced {
  44 protected:
  45   virtual bool configure( SGPropertyNode& cfg_node,
  46                           const std::string& cfg_name,
  47                           SGPropertyNode& prop_root ) = 0;
  48 public:
  49   virtual ~DigitalFilterImplementation() {}
  50   DigitalFilterImplementation();
  51   virtual void   initialize( double initvalue ) {}
  52   virtual double compute( double dt, double input ) = 0;
  53   bool configure( SGPropertyNode& cfg,
  54                   SGPropertyNode& prop_root );
  55
  56   void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
  57
  58 protected:
  59   DigitalFilter * _digitalFilter;
  60 };
  61
  62 /* --------------------------------------------------------------------------------- */
  63 /* --------------------------------------------------------------------------------- */
  64 class GainFilterImplementation : public DigitalFilterImplementation {
  65 protected:
  66   InputValueList _gainInput;
  67   bool configure( SGPropertyNode& cfg_node,
  68                   const std::string& cfg_name,
  69                   SGPropertyNode& prop_root );
  70 public:
  71   GainFilterImplementation() : _gainInput(1.0) {}
  72   double compute(  double dt, double input );
  73 };
  74
  75 class ReciprocalFilterImplementation : public GainFilterImplementation {
  76 public:
  77   double compute(  double dt, double input );
  78 };
  79
  80 class DerivativeFilterImplementation : public GainFilterImplementation {
  81   InputValueList _TfInput;
  82   double _input_1;
  83   bool configure( SGPropertyNode& cfg_node,
  84                   const std::string& cfg_name,
  85                   SGPropertyNode& prop_root );
  86 public:
  87   DerivativeFilterImplementation();
  88   double compute(  double dt, double input );
  89   virtual void initialize( double initvalue );
  90 };
  91
  92 class ExponentialFilterImplementation : public GainFilterImplementation {
  93 protected:
  94   InputValueList _TfInput;
  95   bool configure( SGPropertyNode& cfg_node,
  96                   const std::string& cfg_name,
  97                   SGPropertyNode& prop_root );
  98   bool _isSecondOrder;
  99   double _output_1, _output_2;
 100 public:
 101   ExponentialFilterImplementation();
 102   double compute(  double dt, double input );
 103   virtual void initialize( double initvalue );
 104 };
 105
 106 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
 107 protected:
 108   InputValueList _samplesInput;
 109   double _output_1;
 110   std::deque <double> _inputQueue;
 111   bool configure( SGPropertyNode& cfg_node,
 112                   const std::string& cfg_name,
 113                   SGPropertyNode& prop_root );
 114 public:
 115   MovingAverageFilterImplementation();
 116   double compute(  double dt, double input );
 117   virtual void initialize( double initvalue );
 118 };
 119
 120 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
 121 protected:
 122   double _output_1;
 123   InputValueList _rateOfChangeInput;
 124   bool configure( SGPropertyNode& cfg_node,
 125                   const std::string& cfg_name,
 126                   SGPropertyNode& prop_root );
 127 public:
 128   NoiseSpikeFilterImplementation();
 129   double compute(  double dt, double input );
 130   virtual void initialize( double initvalue );
 131 };
 132
 133 class RateLimitFilterImplementation : public DigitalFilterImplementation {
 134 protected:
 135   double _output_1;
 136   InputValueList _rateOfChangeMax;
 137   InputValueList _rateOfChangeMin ;
 138   bool configure( SGPropertyNode& cfg_node,
 139                   const std::string& cfg_name,
 140                   SGPropertyNode& prop_root );
 141 public:
 142   RateLimitFilterImplementation();
 143   double compute(  double dt, double input );
 144   virtual void initialize( double initvalue );
 145 };
 146
 147 class IntegratorFilterImplementation : public GainFilterImplementation {
 148 protected:
 149   InputValueList _TfInput;
 150   InputValueList _minInput;
 151   InputValueList _maxInput;
 152   double _input_1;
 153   double _output_1;
 154   bool configure( SGPropertyNode& cfg_node,
 155                   const std::string& cfg_name,
 156                   SGPropertyNode& prop_root );
 157 public:
 158   IntegratorFilterImplementation();
 159   double compute(  double dt, double input );
 160   virtual void initialize( double initvalue );
 161 };
 162
 163 class HighPassFilterImplementation : public GainFilterImplementation {
 164 protected:
 165   InputValueList _TfInput;
 166   double _input_1;
 167   double _output_1;
 168   bool configure( SGPropertyNode& cfg_node,
 169                   const std::string& cfg_name,
 170                   SGPropertyNode& prop_root );
 171 public:
 172   HighPassFilterImplementation();
 173   double compute(  double dt, double input );
 174   virtual void initialize( double initvalue );
 175 };
 176 class LeadLagFilterImplementation : public GainFilterImplementation {
 177 protected:
 178   InputValueList _TfaInput;
 179   InputValueList _TfbInput;
 180   double _input_1;
 181   double _output_1;
 182   bool configure( SGPropertyNode& cfg_node,
 183                   const std::string& cfg_name,
 184                   SGPropertyNode& prop_root );
 185 public:
 186   LeadLagFilterImplementation();
 187   double compute(  double dt, double input );
 188   virtual void initialize( double initvalue );
 189 };
 190 /* --------------------------------------------------------------------------------- */
 191 /* --------------------------------------------------------------------------------- */
 192
 193 } // namespace FGXMLAutopilot
 194
 195 using namespace FGXMLAutopilot;
 196
 197 /* --------------------------------------------------------------------------------- */
 198 /* --------------------------------------------------------------------------------- */
 199 DigitalFilterImplementation::DigitalFilterImplementation() :
 200   _digitalFilter(NULL)
 201 {
 202 }
 203
 204 //------------------------------------------------------------------------------
 205 bool DigitalFilterImplementation::configure( SGPropertyNode& cfg,
 206                                              SGPropertyNode& prop_root )
 207 {
 208   for (int i = 0; i < cfg.nChildren(); ++i )
 209   {
 210     SGPropertyNode_ptr child = cfg.getChild(i);
 211
 212     if( configure(*child, child->getNameString(), prop_root) )
 213       continue;
 214   }
 215
 216   return true;
 217 }
 218
 219 /* --------------------------------------------------------------------------------- */
 220 /* --------------------------------------------------------------------------------- */
 221
 222 double GainFilterImplementation::compute(  double dt, double input )
 223 {
 224   return _gainInput.get_value() * input;
 225 }
 226
 227 bool GainFilterImplementation::configure( SGPropertyNode& cfg_node,
 228                                           const std::string& cfg_name,
 229                                           SGPropertyNode& prop_root )
 230 {
 231   if (cfg_name == "gain" ) {
 232     _gainInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 233     return true;
 234   }
 235
 236   return false;
 237 }
 238
 239 /* --------------------------------------------------------------------------------- */
 240 /* --------------------------------------------------------------------------------- */
 241
 242 double ReciprocalFilterImplementation::compute(  double dt, double input )
 243 {
 244   if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
 245     return SGLimitsd::max();
 246
 247   return _gainInput.get_value() / input;
 248
 249 }
 250
 251 /* --------------------------------------------------------------------------------- */
 252 /* --------------------------------------------------------------------------------- */
 253
 254 DerivativeFilterImplementation::DerivativeFilterImplementation() :
 255   _input_1(0.0)
 256 {
 257 }
 258
 259 void DerivativeFilterImplementation::initialize( double initvalue )
 260 {
 261   _input_1 = initvalue;
 262 }
 263
 264 //------------------------------------------------------------------------------
 265 bool DerivativeFilterImplementation::configure( SGPropertyNode& cfg_node,
 266                                                 const std::string& cfg_name,
 267                                                 SGPropertyNode& prop_root )
 268 {
 269   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 270     return true;
 271
 272   if (cfg_name == "filter-time" ) {
 273     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 274     return true;
 275   }
 276
 277   return false;
 278 }
 279
 280 double DerivativeFilterImplementation::compute(  double dt, double input )
 281 {
 282   double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
 283   _input_1 = input;
 284   return output;
 285
 286 }
 287
 288 /* --------------------------------------------------------------------------------- */
 289 /* --------------------------------------------------------------------------------- */
 290
 291 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
 292   _output_1(0.0)
 293 {
 294 }
 295
 296 void MovingAverageFilterImplementation::initialize( double initvalue )
 297 {
 298   _output_1 = initvalue;
 299 }
 300
 301 double MovingAverageFilterImplementation::compute(  double dt, double input )
 302 {
 303   typedef std::deque<double>::size_type size_type;
 304   size_type samples = _samplesInput.get_value();
 305
 306   if (_inputQueue.size() != samples) {
 307     // For constant size filters, this code executed once.
 308     bool shrunk = _inputQueue.size() > samples;
 309     _inputQueue.resize(samples, _output_1);
 310     if (shrunk) {
 311       _output_1 = 0.0;
 312       for (size_type ii = 0; ii < samples; ii++)
 313       _output_1 += _inputQueue[ii];
 314       _output_1 /= samples;
 315     }
 316   }
 317
 318   double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
 319
 320   _output_1 = output_0;
 321   _inputQueue.pop_back();
 322   _inputQueue.push_front(input);
 323   return output_0;
 324 }
 325
 326 bool MovingAverageFilterImplementation::configure( SGPropertyNode& cfg_node,
 327                                                    const std::string& cfg_name,
 328                                                    SGPropertyNode& prop_root )
 329 {
 330   if (cfg_name == "samples" ) {
 331     _samplesInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 332     return true;
 333   }
 334
 335   return false;
 336 }
 337
 338 /* --------------------------------------------------------------------------------- */
 339 /* --------------------------------------------------------------------------------- */
 340
 341 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
 342   _output_1(0.0)
 343 {
 344 }
 345
 346 void NoiseSpikeFilterImplementation::initialize( double initvalue )
 347 {
 348   _output_1 = initvalue;
 349 }
 350
 351 double NoiseSpikeFilterImplementation::compute(  double dt, double input )
 352 {
 353   double delta = input - _output_1;
 354   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 355
 356   double maxChange = _rateOfChangeInput.get_value() * dt;
 357   const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 358   if( periodical ) delta = periodical->normalizeSymmetric( delta );
 359
 360   if( fabs(delta) <= maxChange )
 361     return (_output_1 = input);
 362   else
 363     return (_output_1 = _output_1 + copysign( maxChange, delta ));
 364 }
 365
 366 //------------------------------------------------------------------------------
 367 bool NoiseSpikeFilterImplementation::configure( SGPropertyNode& cfg_node,
 368                                                 const std::string& cfg_name,
 369                                                 SGPropertyNode& prop_root )
 370 {
 371   if (cfg_name == "max-rate-of-change" ) {
 372     _rateOfChangeInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 373     return true;
 374   }
 375
 376   return false;
 377 }
 378
 379 /* --------------------------------------------------------------------------------- */
 380
 381 RateLimitFilterImplementation::RateLimitFilterImplementation() :
 382   _output_1(0.0)
 383 {
 384 }
 385
 386 void RateLimitFilterImplementation::initialize( double initvalue )
 387 {
 388   _output_1 = initvalue;
 389 }
 390
 391 double RateLimitFilterImplementation::compute(  double dt, double input )
 392 {
 393   double delta = input - _output_1;
 394   double output;
 395
 396   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 397
 398   double maxChange = _rateOfChangeMax.get_value() * dt;
 399   double minChange = _rateOfChangeMin.get_value() * dt;
 400 //  const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 401 //  if( periodical ) delta = periodical->normalizeSymmetric( delta );
 402
 403   output = input;
 404   if(delta >= maxChange ) output = _output_1 + maxChange;
 405   if(delta <= minChange ) output = _output_1 + minChange;
 406   _output_1 = output;
 407
 408   return (output);
 409 }
 410
 411 bool RateLimitFilterImplementation::configure( SGPropertyNode& cfg_node,
 412                                                const std::string& cfg_name,
 413                                                SGPropertyNode& prop_root )
 414 {
 415   if (cfg_name == "max-rate-of-change" ) {
 416     _rateOfChangeMax.push_back( new InputValue(prop_root, cfg_node, 1) );
 417     return true;
 418   }
 419   if (cfg_name == "min-rate-of-change" ) {
 420     _rateOfChangeMin.push_back( new InputValue(prop_root, cfg_node, 1) );
 421     return true;
 422   }
 423
 424   return false;
 425 }
 426
 427 /* --------------------------------------------------------------------------------- */
 428 /* --------------------------------------------------------------------------------- */
 429
 430 ExponentialFilterImplementation::ExponentialFilterImplementation()
 431   : _isSecondOrder(false),
 432     _output_1(0.0),
 433     _output_2(0.0)
 434 {
 435 }
 436
 437 void ExponentialFilterImplementation::initialize( double initvalue )
 438 {
 439   _output_1 = _output_2 = initvalue;
 440 }
 441
 442 double ExponentialFilterImplementation::compute(  double dt, double input )
 443 {
 444   input = GainFilterImplementation::compute( dt, input );
 445   double tf = _TfInput.get_value();
 446
 447   double output_0;
 448
 449   // avoid negative filter times
 450   // and div by zero if -tf == dt
 451
 452   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 453
 454   if(_isSecondOrder) {
 455     output_0 = alpha * alpha * input +
 456                2 * (1 - alpha) * _output_1 -
 457               (1 - alpha) * (1 - alpha) * _output_2;
 458   } else {
 459     output_0 = alpha * input + (1 - alpha) * _output_1;
 460   }
 461   _output_2 = _output_1;
 462   return (_output_1 = output_0);
 463 }
 464
 465 //------------------------------------------------------------------------------
 466 bool ExponentialFilterImplementation::configure( SGPropertyNode& cfg_node,
 467                                                  const std::string& cfg_name,
 468                                                  SGPropertyNode& prop_root )
 469 {
 470   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 471     return true;
 472
 473   if (cfg_name == "filter-time" ) {
 474     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 475     return true;
 476   }
 477
 478   if (cfg_name == "type" ) {
 479     string type(cfg_node.getStringValue());
 480     _isSecondOrder = type == "double-exponential";
 481   }
 482
 483   return false;
 484 }
 485
 486 /* --------------------------------------------------------------------------------- */
 487
 488 IntegratorFilterImplementation::IntegratorFilterImplementation() :
 489   _input_1(0.0),
 490   _output_1(0.0)
 491 {
 492 }
 493
 494 void IntegratorFilterImplementation::initialize( double initvalue )
 495 {
 496   _input_1 = _output_1 = initvalue;
 497 }
 498
 499 //------------------------------------------------------------------------------
 500 bool IntegratorFilterImplementation::configure( SGPropertyNode& cfg_node,
 501                                                 const std::string& cfg_name,
 502                                                 SGPropertyNode& prop_root )
 503 {
 504   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 505     return true;
 506
 507   if (cfg_name == "u_min" ) {
 508     _minInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 509     return true;
 510   }
 511   if (cfg_name == "u_max" ) {
 512     _maxInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 513     return true;
 514   }
 515   return false;
 516 }
 517
 518 double IntegratorFilterImplementation::compute(  double dt, double input )
 519 {
 520   double output = _output_1 + input *  _gainInput.get_value() * dt;
 521   double u_min = _minInput.get_value();
 522   double u_max = _maxInput.get_value();
 523   if (output >= u_max) output = u_max; // clamping inside "::compute" prevents integrator wind-up
 524   if (output <= u_min) output = u_min;
 525   _input_1 = input;
 526   _output_1 = output;
 527   return output;
 528
 529 }
 530
 531 /* --------------------------------------------------------------------------------- */
 532
 533 HighPassFilterImplementation::HighPassFilterImplementation() :
 534   _input_1(0.0),
 535   _output_1(0.0)
 536
 537 {
 538 }
 539
 540 void HighPassFilterImplementation::initialize( double initvalue )
 541 {
 542   _input_1 = initvalue;
 543   _output_1 = initvalue;
 544 }
 545
 546 double HighPassFilterImplementation::compute(  double dt, double input )
 547 {
 548   input = GainFilterImplementation::compute( dt, input );
 549   double tf = _TfInput.get_value();
 550
 551   double output;
 552
 553   // avoid negative filter times
 554   // and div by zero if -tf == dt
 555
 556   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 557   output = (1 - alpha) * (input - _input_1 +  _output_1);
 558   _input_1 = input;
 559   _output_1 = output;
 560   return output;
 561 }
 562
 563 //------------------------------------------------------------------------------
 564 bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
 565                                               const std::string& cfg_name,
 566                                               SGPropertyNode& prop_root )
 567 {
 568   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 569     return true;
 570
 571   if (cfg_name == "filter-time" ) {
 572     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 573     return true;
 574   }
 575
 576   return false;
 577 }
 578
 579 /* --------------------------------------------------------------------------------- */
 580
 581 LeadLagFilterImplementation::LeadLagFilterImplementation() :
 582   _input_1(0.0),
 583   _output_1(0.0)
 584
 585 {
 586 }
 587
 588 void LeadLagFilterImplementation::initialize( double initvalue )
 589 {
 590   _input_1 = initvalue;
 591   _output_1 = initvalue;
 592 }
 593
 594 double LeadLagFilterImplementation::compute(  double dt, double input )
 595 {
 596   input = GainFilterImplementation::compute( dt, input );
 597   double tfa = _TfaInput.get_value();
 598   double tfb = _TfbInput.get_value();
 599
 600   double output;
 601
 602   // avoid negative filter times
 603   // and div by zero if -tf == dt
 604
 605   double alpha = tfa > 0.0 ? 1 / ((tfa/dt) + 1) : 1.0;
 606   double beta = tfb > 0.0 ? 1 / ((tfb/dt) + 1) : 1.0;
 607   output = (1 - beta) * (input / (1 - alpha) - _input_1 + _output_1);
 608   _input_1 = input;
 609   _output_1 = output;
 610   return output;
 611 }
 612
 613 //------------------------------------------------------------------------------
 614 bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node,
 615                                              const std::string& cfg_name,
 616                                              SGPropertyNode& prop_root )
 617 {
 618   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 619     return true;
 620
 621   if (cfg_name == "filter-time-a" ) {
 622     _TfaInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 623     return true;
 624   }
 625   if (cfg_name == "filter-time-b" ) {
 626     _TfbInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 627     return true;
 628   }
 629   return false;
 630 }
 631 /* -------------------------------------------------------------------------- */
 632 /* Digital Filter Component Implementation                                    */
 633 /* -------------------------------------------------------------------------- */
 634
 635 DigitalFilter::DigitalFilter() :
 636     AnalogComponent(),
 637     _initializeTo(INITIALIZE_INPUT)
 638 {
 639 }
 640
 641 DigitalFilter::~DigitalFilter()
 642 {
 643 }
 644
 645
 646 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
 647
 648 //------------------------------------------------------------------------------
 649 bool DigitalFilter::configure( SGPropertyNode& cfg_node,
 650                                const std::string& cfg_name,
 651                                SGPropertyNode& prop_root )
 652 {
 653   if( componentForge.empty() ) {
 654     componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
 655     componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 656     componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 657     componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
 658     componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
 659     componentForge["rate-limit"] = new CreateAndConfigureFunctor<RateLimitFilterImplementation,DigitalFilterImplementation>();
 660     componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
 661     componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
 662     componentForge["high-pass"] = new CreateAndConfigureFunctor<HighPassFilterImplementation,DigitalFilterImplementation>();
 663     componentForge["lead-lag"] = new CreateAndConfigureFunctor<LeadLagFilterImplementation,DigitalFilterImplementation>();
 664     componentForge["integrator"] = new CreateAndConfigureFunctor<IntegratorFilterImplementation,DigitalFilterImplementation>();
 665   }
 666
 667   SG_LOG(SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << cfg_name << ")");
 668
 669   if( AnalogComponent::configure(cfg_node, cfg_name, prop_root) )
 670     return true;
 671
 672   if (cfg_name == "type" ) {
 673     string type( cfg_node.getStringValue() );
 674     if( componentForge.count(type) == 0 ) {
 675       SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
 676       return true;
 677     }
 678     _implementation = (*componentForge[type])(*cfg_node.getParent(), prop_root);
 679     _implementation->setDigitalFilter( this );
 680     return true;
 681   }
 682
 683   if( cfg_name == "initialize-to" ) {
 684     string s( cfg_node.getStringValue() );
 685     if( s == "input" ) {
 686       _initializeTo = INITIALIZE_INPUT;
 687     } else if( s == "output" ) {
 688       _initializeTo = INITIALIZE_OUTPUT;
 689     } else if( s == "none" ) {
 690       _initializeTo = INITIALIZE_NONE;
 691     } else {
 692       SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
 693     }
 694     return true;
 695   }
 696
 697   SG_LOG
 698   (
 699     SG_AUTOPILOT,
 700     SG_BULK,
 701     "DigitalFilter::configure(" << cfg_name << ") [unhandled]"
 702   );
 703   return false; // not handled by us, let the base class try
 704 }
 705
 706 void DigitalFilter::update( bool firstTime, double dt)
 707 {
 708   if( _implementation == NULL ) return;
 709
 710   if( firstTime ) {
 711     switch( _initializeTo ) {
 712
 713       case INITIALIZE_INPUT:
 714         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
 715         _implementation->initialize( _valueInput.get_value() );
 716         break;
 717
 718       case INITIALIZE_OUTPUT:
 719         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
 720         _implementation->initialize( get_output_value() );
 721         break;
 722
 723       default:
 724         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
 725         break;
 726     }
 727   }
 728
 729   double input = _valueInput.get_value() - _referenceInput.get_value();
 730   double output = _implementation->compute( dt, input );
 731
 732   set_output_value( output );
 733
 734   if(_debug) {
 735     cout << "input:" << input
 736          << "\toutput:" << output << endl;
 737   }
 738 }