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"
28 namespace FGXMLAutopilot {
30 /* --------------------------------------------------------------------------------- */
31 /* --------------------------------------------------------------------------------- */
32 class GainFilterImplementation : public DigitalFilterImplementation {
34 InputValueList _gainInput;
35 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
37 GainFilterImplementation() : _gainInput(1.0) {}
38 double compute( double dt, double input );
41 class ReciprocalFilterImplementation : public GainFilterImplementation {
43 double compute( double dt, double input );
46 class DerivativeFilterImplementation : public GainFilterImplementation {
47 InputValueList _TfInput;
49 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
51 DerivativeFilterImplementation();
52 double compute( double dt, double input );
55 class ExponentialFilterImplementation : public GainFilterImplementation {
57 InputValueList _TfInput;
58 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
60 double output_1, output_2;
62 ExponentialFilterImplementation();
63 double compute( double dt, double input );
64 virtual void initialize( double output );
67 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
69 InputValueList _samplesInput;
71 std::deque <double> _inputQueue;
72 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
74 MovingAverageFilterImplementation();
75 double compute( double dt, double input );
76 virtual void initialize( double output );
79 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
82 InputValueList _rateOfChangeInput;
83 bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
85 NoiseSpikeFilterImplementation();
86 double compute( double dt, double input );
87 virtual void initialize( double output );
90 /* --------------------------------------------------------------------------------- */
91 /* --------------------------------------------------------------------------------- */
93 } // namespace FGXMLAutopilot
95 using namespace FGXMLAutopilot;
97 /* --------------------------------------------------------------------------------- */
98 /* --------------------------------------------------------------------------------- */
100 bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
102 for (int i = 0; i < configNode->nChildren(); ++i ) {
103 SGPropertyNode_ptr prop;
105 SGPropertyNode_ptr child = configNode->getChild(i);
106 string cname(child->getName());
108 if( configure( cname, child ) )
111 } // for configNode->nChildren()
116 /* --------------------------------------------------------------------------------- */
117 /* --------------------------------------------------------------------------------- */
119 double GainFilterImplementation::compute( double dt, double input )
121 return _gainInput.get_value() * input;
124 bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
126 if (nodeName == "gain" ) {
127 _gainInput.push_back( new InputValue( configNode, 1 ) );
134 /* --------------------------------------------------------------------------------- */
135 /* --------------------------------------------------------------------------------- */
137 double ReciprocalFilterImplementation::compute( double dt, double input )
139 if( input >= -SGLimitsd::min() || input <= SGLimitsd::min() )
140 return SGLimitsd::max();
142 return _gainInput.get_value() / input;
146 /* --------------------------------------------------------------------------------- */
147 /* --------------------------------------------------------------------------------- */
149 DerivativeFilterImplementation::DerivativeFilterImplementation() :
154 bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
156 if( GainFilterImplementation::configure( nodeName, configNode ) )
159 if (nodeName == "filter-time" ) {
160 _TfInput.push_back( new InputValue( configNode, 1 ) );
167 double DerivativeFilterImplementation::compute( double dt, double input )
169 double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
175 /* --------------------------------------------------------------------------------- */
176 /* --------------------------------------------------------------------------------- */
178 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
183 void MovingAverageFilterImplementation::initialize( double output )
188 double MovingAverageFilterImplementation::compute( double dt, double input )
190 std::deque<double>::size_type samples = _samplesInput.get_value();
191 _inputQueue.resize(samples+1, 0.0);
193 double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
195 _output_1 = output_0;
196 _inputQueue.push_front(input);
200 bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
202 if (nodeName == "samples" ) {
203 _samplesInput.push_back( new InputValue( configNode, 1 ) );
210 /* --------------------------------------------------------------------------------- */
211 /* --------------------------------------------------------------------------------- */
213 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
218 void NoiseSpikeFilterImplementation::initialize( double output )
223 double NoiseSpikeFilterImplementation::compute( double dt, double input )
225 double maxChange = _rateOfChangeInput.get_value() * dt;
227 double output_0 = _output_1;
229 if (_output_1 - input > maxChange) {
230 output_0 = _output_1 - maxChange;
231 } else if( _output_1 - input < -maxChange ) {
232 output_0 = _output_1 + maxChange;
233 } else if (fabs(input - _output_1) <= maxChange) {
236 _output_1 = output_0;
240 bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
242 if (nodeName == "max-rate-of-change" ) {
243 _rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
250 /* --------------------------------------------------------------------------------- */
251 /* --------------------------------------------------------------------------------- */
253 ExponentialFilterImplementation::ExponentialFilterImplementation()
254 : _isSecondOrder(false),
260 void ExponentialFilterImplementation::initialize( double output )
262 output_1 = output_2 = output;
265 double ExponentialFilterImplementation::compute( double dt, double input )
267 input = GainFilterImplementation::compute( dt, input );
270 double alpha = 1 / ((_TfInput.get_value()/dt) + 1);
273 output_0 = alpha * alpha * input +
274 2 * (1 - alpha) * output_1 -
275 (1 - alpha) * (1 - alpha) * output_2;
277 output_0 = alpha * input + (1 - alpha) * output_1;
280 return (output_1 = output_0);
283 bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
285 if( GainFilterImplementation::configure( nodeName, configNode ) )
288 if (nodeName == "filter-time" ) {
289 _TfInput.push_back( new InputValue( configNode, 1 ) );
293 if (nodeName == "type" ) {
294 string type(configNode->getStringValue());
295 _isSecondOrder = type == "double-exponential";
301 /* --------------------------------------------------------------------------------- */
302 /* Digital Filter Component Implementation */
303 /* --------------------------------------------------------------------------------- */
305 DigitalFilter::DigitalFilter() :
310 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
312 bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
314 if( componentForge.empty() ) {
315 componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
316 componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
317 componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
318 componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
319 componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
320 componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
321 componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
324 SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
325 if( AnalogComponent::configure( nodeName, configNode ) )
328 if (nodeName == "type" ) {
329 string type( configNode->getStringValue() );
330 if( componentForge.count(type) == 0 ) {
331 SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
334 _implementation = (*componentForge[type])( configNode->getParent() );
338 SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
339 return false; // not handled by us, let the base class try
342 void DigitalFilter::update( bool firstTime, double dt)
344 if( _implementation == NULL ) return;
347 _implementation->initialize( get_output_value() );
349 double input = _valueInput.get_value() - _referenceInput.get_value();
350 double output = _implementation->compute( dt, input );
352 set_output_value( output );
355 cout << "input:" << input
356 << "\toutput:" << output << endl;