1 // digitalfilter.cxx - a selection of digital filters
3 // Written by Torsten Dreyer
4 // Based heavily on work created by Curtis Olson, started January 2004.
6 // Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
7 // Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
9 // This program is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU General Public License as
11 // published by the Free Software Foundation; either version 2 of the
12 // License, or (at your option) any later version.
14 // This program is distributed in the hope that it will be useful, but
15 // WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 // General Public License for more details.
19 // You should have received a copy of the GNU General Public License
20 // along with this program; if not, write to the Free Software
21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
24 #include "digitalfilter.hxx"
25 #include "functor.hxx"
33 namespace FGXMLAutopilot {
35 /* --------------------------------------------------------------------------------- */
36 /* --------------------------------------------------------------------------------- */
37 class GainFilterImplementation : public DigitalFilterImplementation {
39 InputValueList _gainInput;
40 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
42 GainFilterImplementation() : _gainInput(1.0) {}
43 double compute( double dt, double input );
46 class ReciprocalFilterImplementation : public GainFilterImplementation {
48 double compute( double dt, double input );
51 class DerivativeFilterImplementation : public GainFilterImplementation {
52 InputValueList _TfInput;
54 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
56 DerivativeFilterImplementation();
57 double compute( double dt, double input );
60 class ExponentialFilterImplementation : public GainFilterImplementation {
62 InputValueList _TfInput;
63 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
65 double output_1, output_2;
67 ExponentialFilterImplementation();
68 double compute( double dt, double input );
69 virtual void initialize( double output );
72 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
74 InputValueList _samplesInput;
76 std::deque <double> _inputQueue;
77 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
79 MovingAverageFilterImplementation();
80 double compute( double dt, double input );
81 virtual void initialize( double output );
84 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
87 InputValueList _rateOfChangeInput;
88 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
90 NoiseSpikeFilterImplementation();
91 double compute( double dt, double input );
92 virtual void initialize( double output );
95 /* --------------------------------------------------------------------------------- */
96 /* --------------------------------------------------------------------------------- */
98 } // namespace FGXMLAutopilot
100 using namespace FGXMLAutopilot;
102 /* --------------------------------------------------------------------------------- */
103 /* --------------------------------------------------------------------------------- */
105 bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
107 for (int i = 0; i < configNode->nChildren(); ++i ) {
108 SGPropertyNode_ptr prop;
110 SGPropertyNode_ptr child = configNode->getChild(i);
111 string cname(child->getName());
113 if( configure( cname, child ) )
116 } // for configNode->nChildren()
121 /* --------------------------------------------------------------------------------- */
122 /* --------------------------------------------------------------------------------- */
124 double GainFilterImplementation::compute( double dt, double input )
126 return _gainInput.get_value() * input;
129 bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
131 if (nodeName == "gain" ) {
132 _gainInput.push_back( new InputValue( configNode, 1 ) );
139 /* --------------------------------------------------------------------------------- */
140 /* --------------------------------------------------------------------------------- */
142 double ReciprocalFilterImplementation::compute( double dt, double input )
144 if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
145 return SGLimitsd::max();
147 return _gainInput.get_value() / input;
151 /* --------------------------------------------------------------------------------- */
152 /* --------------------------------------------------------------------------------- */
154 DerivativeFilterImplementation::DerivativeFilterImplementation() :
159 bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
161 if( GainFilterImplementation::configure( nodeName, configNode ) )
164 if (nodeName == "filter-time" ) {
165 _TfInput.push_back( new InputValue( configNode, 1 ) );
172 double DerivativeFilterImplementation::compute( double dt, double input )
174 double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
180 /* --------------------------------------------------------------------------------- */
181 /* --------------------------------------------------------------------------------- */
183 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
188 void MovingAverageFilterImplementation::initialize( double output )
193 double MovingAverageFilterImplementation::compute( double dt, double input )
195 std::deque<double>::size_type samples = _samplesInput.get_value();
196 _inputQueue.resize(samples+1, 0.0);
198 double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
200 _output_1 = output_0;
201 _inputQueue.push_front(input);
205 bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
207 if (nodeName == "samples" ) {
208 _samplesInput.push_back( new InputValue( configNode, 1 ) );
215 /* --------------------------------------------------------------------------------- */
216 /* --------------------------------------------------------------------------------- */
218 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
223 void NoiseSpikeFilterImplementation::initialize( double output )
228 double NoiseSpikeFilterImplementation::compute( double dt, double input )
230 double maxChange = _rateOfChangeInput.get_value() * dt;
232 double output_0 = _output_1;
234 if (_output_1 - input > maxChange) {
235 output_0 = _output_1 - maxChange;
236 } else if( _output_1 - input < -maxChange ) {
237 output_0 = _output_1 + maxChange;
238 } else if (fabs(input - _output_1) <= maxChange) {
241 _output_1 = output_0;
245 bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
247 if (nodeName == "max-rate-of-change" ) {
248 _rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
255 /* --------------------------------------------------------------------------------- */
256 /* --------------------------------------------------------------------------------- */
258 ExponentialFilterImplementation::ExponentialFilterImplementation()
259 : _isSecondOrder(false),
265 void ExponentialFilterImplementation::initialize( double output )
267 output_1 = output_2 = output;
270 double ExponentialFilterImplementation::compute( double dt, double input )
272 input = GainFilterImplementation::compute( dt, input );
273 double tf = _TfInput.get_value();
277 // avoid negative filter times
278 // and div by zero if -tf == dt
280 double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
283 output_0 = alpha * alpha * input +
284 2 * (1 - alpha) * output_1 -
285 (1 - alpha) * (1 - alpha) * output_2;
287 output_0 = alpha * input + (1 - alpha) * output_1;
290 return (output_1 = output_0);
293 bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
295 if( GainFilterImplementation::configure( nodeName, configNode ) )
298 if (nodeName == "filter-time" ) {
299 _TfInput.push_back( new InputValue( configNode, 1 ) );
303 if (nodeName == "type" ) {
304 string type(configNode->getStringValue());
305 _isSecondOrder = type == "double-exponential";
311 /* --------------------------------------------------------------------------------- */
312 /* Digital Filter Component Implementation */
313 /* --------------------------------------------------------------------------------- */
315 DigitalFilter::DigitalFilter() :
317 _initializeTo(INITIALIZE_INPUT)
321 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
323 bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
325 if( componentForge.empty() ) {
326 componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
327 componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
328 componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
329 componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
330 componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
331 componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
332 componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
335 SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
336 if( AnalogComponent::configure( nodeName, configNode ) )
339 if (nodeName == "type" ) {
340 string type( configNode->getStringValue() );
341 if( componentForge.count(type) == 0 ) {
342 SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
345 _implementation = (*componentForge[type])( configNode->getParent() );
349 if( nodeName == "initialize-to" ) {
350 string s( configNode->getStringValue() );
352 _initializeTo = INITIALIZE_INPUT;
353 } else if( s == "output" ) {
354 _initializeTo = INITIALIZE_OUTPUT;
355 } else if( s == "none" ) {
356 _initializeTo = INITIALIZE_NONE;
358 SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
363 SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
364 return false; // not handled by us, let the base class try
367 void DigitalFilter::update( bool firstTime, double dt)
369 if( _implementation == NULL ) return;
372 switch( _initializeTo ) {
374 case INITIALIZE_INPUT:
375 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
376 _implementation->initialize( _valueInput.get_value() );
379 case INITIALIZE_OUTPUT:
380 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
381 _implementation->initialize( get_output_value() );
385 SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
390 double input = _valueInput.get_value() - _referenceInput.get_value();
391 double output = _implementation->compute( dt, input );
393 set_output_value( output );
396 cout << "input:" << input
397 << "\toutput:" << output << endl;