Line data Source code
1 : /** \file leakyIntegrator.hpp
2 : * \author Jared R. Males (jaredmales@gmail.com)
3 : * \brief Declares and defines the leaky integrator controller class for AO control.
4 : * \ingroup mxAO_sim_files
5 : *
6 : */
7 :
8 : #ifndef __leakyIntegrator_hpp__
9 : #define __leakyIntegrator_hpp__
10 :
11 : #include <fstream>
12 : #include <string>
13 : #include <vector>
14 :
15 : #include <Eigen/Dense>
16 :
17 : #include "../../error/mxErrorOld.hpp"
18 : #include "../../ioutils/stringUtils.hpp"
19 :
20 : #include "wavefront.hpp"
21 :
22 : namespace mx
23 : {
24 : namespace AO
25 : {
26 : namespace sim
27 : {
28 :
29 : template <typename _realT>
30 : struct wfMeasurement
31 : {
32 : typedef _realT realT;
33 :
34 : realT iterNo;
35 :
36 : typedef Eigen::Array<realT, Eigen::Dynamic, Eigen::Dynamic> commandT;
37 :
38 : commandT measurement;
39 : };
40 :
41 : /// Implements the leaky integrator controller.
42 : /**
43 : * \tparam _realT is the floating point type for all calculations.
44 : */
45 : template <typename _realT>
46 : class leakyIntegrator
47 : {
48 :
49 : public:
50 : /// The real data type
51 : typedef _realT realT;
52 :
53 : /// The wavefront data type
54 : typedef wavefront<realT> wavefrontT;
55 :
56 : /// The command type
57 : typedef wfMeasurement<realT> commandT;
58 :
59 : /// The image type, used here as a general storage array
60 : typedef Eigen::Array<realT, Eigen::Dynamic, Eigen::Dynamic> imageT;
61 :
62 : /// Default c'tor.
63 : leakyIntegrator();
64 :
65 : protected:
66 : int _nModes; ///< The number of modes being filtered.
67 :
68 : bool _openLoop; ///< If true, then commands are not integrated.
69 :
70 : int _closingDelay; ///< If > 0, then the gains are ramped linearly up to this value. Default = 0.
71 :
72 : int _lowOrders; ///< If > 0, then this sets the maximum mode number which is filtered. All remaining modes are set
73 : ///< to 0. Default = 0.
74 :
75 : imageT _gains; ///< Column-vector of gains
76 :
77 : imageT _leaks; ///< Column-vector of leaks
78 :
79 : imageT _commands; ///< Column-vector past commands
80 :
81 : public:
82 : int _lowOrderDelay;
83 :
84 : /// Allocate and initialize all state.
85 : /**
86 : * \returns 0 on success, a negative integer otherwise.
87 : */
88 : int initialize( int nModes /**< [in] the number of modes to be filtered */ );
89 :
90 : /// Get the number of modes
91 : /** nModes is only set by calling initialize.
92 : *
93 : * \returns the current value of _nModes
94 : */
95 : int nModes();
96 :
97 : /// Set the _openLoop flag.
98 : /** If _openLoop is true, then commands are not filtered.
99 : *
100 : * \returns 0 on success, a negative integer on error.
101 : */
102 : int openLoop( bool ol /**< The new value of _openLoop */ );
103 :
104 : /// Get the value of the _openLoop flag.
105 : /**
106 : * \returns the current value of _openLoop.
107 : */
108 : bool openLoop();
109 :
110 : /// Set _closingDelay.
111 : /** If _closingDelay > 0, then the gains are ramped linearly up to this value.
112 : *
113 : * \returns 0 on success, a negative integer on error.
114 : */
115 : int closingDelay( int cd /**< The new value of _closingDelay */ );
116 :
117 : /// Get the value of the _closingDelay.
118 : /**
119 : * \returns the current value of _closingDelay.
120 : */
121 : int closingDelay();
122 :
123 : /// Set _lowOrders.
124 : /** If _lowOrders > 0, then this sets the maximum mode number which is filtered. All remaining modes are set to 0.
125 : *
126 : * \returns 0 on success, a negative integer on error.
127 : */
128 : int lowOrders( int lo /**< The new value of _lowOrders */ );
129 :
130 : /// Get the value of the _lowOrders.
131 : /**
132 : * \returns the current value of _lowOrders.
133 : */
134 : int lowOrders();
135 :
136 : /// Get the gain for a single mode.
137 : /** \returns the gain value if mode exists.
138 : */
139 : realT gain( int i /**< [in] the mode number*/ );
140 :
141 : /// Set the gain for a single mode.
142 : /**
143 : * \returns 0 on success, negative number on error.
144 : */
145 : int gain( int i, ///< [in] the mode number
146 : realT g ///< [in] the new gain value
147 : );
148 :
149 : /// Set the gain for all modes to a single value
150 : /**
151 : * \returns 0 on success, negative number on error.
152 : */
153 : int gains( realT g /**< [in] the new gain value*/ );
154 :
155 : /// Set the gains for all modes, using a vector to specify each gain
156 : /** The vector must be exactly as long as _nModes.
157 : *
158 : * \returns 0 on success, negative number on error.
159 : */
160 : int gains( const std::vector<realT> &vgains /**< [in] vector of gains.*/ );
161 :
162 : /// Set the gains for all modes, using a file to specify each gain
163 : /** The file format is a simple ASCII single column, with 1 gain per line.
164 : * Must be exactly as long as _nModes.
165 : *
166 : * \returns 0 on success, negative number on error.
167 : */
168 : int gains( const std::string &ogainf /**< [in] the name of the file, full path */ );
169 :
170 : /// Set the leak for a single mode.
171 : /**
172 : * \returns 0 on success, negative number on error.
173 : */
174 : int leak( int i, ///< [in] the mode number
175 : realT l ///< [in] the new leak value for this mode
176 : );
177 :
178 : /// Set a leak for all modes.
179 : /**
180 : * \returns 0 on success, negative number on error.
181 : */
182 : int leaks( realT l /**< [in] the new leak value to set for all modes */ );
183 :
184 : /// Allocate the provided command structures
185 : /** Used by the calling system to allocate the commands being passed between components.
186 : *
187 : * \returns 0 on success, negative number on error.
188 : */
189 : int initMeasurements( commandT &filtAmps, ///< The structure to contain the filtered commands
190 : commandT &rawAmps ///< The structure to contain the raw commands
191 : );
192 :
193 : /// Apply the leaky integrator
194 : /**
195 : * \returns 0 on success, negative number on error.
196 : */
197 : int filterCommands( commandT &filtAmps, ///< [out] the filtered commands
198 : commandT &rawAmps, ///< [in] the raw commands
199 : int iterNo ///< [in] The current iteration number
200 : );
201 : };
202 :
203 : template <typename realT>
204 1 : leakyIntegrator<realT>::leakyIntegrator()
205 : {
206 1 : _nModes = 0;
207 :
208 1 : _openLoop = false;
209 :
210 1 : _closingDelay = 0;
211 :
212 1 : _lowOrders = 0;
213 1 : _lowOrderDelay = 0;
214 1 : }
215 :
216 : template <typename realT>
217 1 : int leakyIntegrator<realT>::initialize( int nModes )
218 : {
219 1 : _nModes = nModes;
220 :
221 1 : _gains.resize( 1, nModes );
222 1 : _leaks.resize( 1, nModes );
223 1 : _commands.resize( 1, nModes );
224 :
225 1 : _gains.setZero();
226 :
227 1 : _leaks.setZero();
228 :
229 1 : _commands.setZero();
230 :
231 1 : return 0;
232 : }
233 :
234 : template <typename realT>
235 0 : int leakyIntegrator<realT>::nModes()
236 : {
237 0 : return _nModes;
238 : }
239 :
240 : template <typename realT>
241 0 : int leakyIntegrator<realT>::openLoop( bool ol )
242 : {
243 0 : _openLoop = ol;
244 0 : return 0;
245 : }
246 :
247 : template <typename realT>
248 0 : bool leakyIntegrator<realT>::openLoop()
249 : {
250 0 : return _openLoop;
251 : }
252 :
253 : template <typename realT>
254 0 : int leakyIntegrator<realT>::closingDelay( int cd )
255 : {
256 0 : _closingDelay = cd;
257 :
258 0 : return 0;
259 : }
260 :
261 : template <typename realT>
262 0 : int leakyIntegrator<realT>::closingDelay()
263 : {
264 0 : return _closingDelay;
265 : }
266 :
267 : template <typename realT>
268 0 : int leakyIntegrator<realT>::lowOrders( int lo )
269 : {
270 0 : _lowOrders = lo;
271 :
272 0 : return 0;
273 : }
274 :
275 : template <typename realT>
276 0 : int leakyIntegrator<realT>::lowOrders()
277 : {
278 0 : return _lowOrders;
279 : }
280 :
281 : template <typename realT>
282 0 : realT leakyIntegrator<realT>::gain( int i )
283 : {
284 0 : if( i < 0 || i >= _nModes )
285 : {
286 0 : mxError( "leakyIntegrator::gain", MXE_INVALIDARG, "mode index out of range" );
287 0 : return 0; ///\retval 0 if the mode doesn't exist.
288 : }
289 :
290 0 : return _gains( i );
291 : }
292 :
293 : template <typename realT>
294 0 : int leakyIntegrator<realT>::gain( int i, realT g )
295 : {
296 0 : if( i < 0 || i >= _nModes )
297 : {
298 0 : mxError( "leakyIntegrator::gain", MXE_INVALIDARG, "mode index out of range" );
299 0 : return 0; ///\retval 0 if the mode doesn't exist.
300 : }
301 :
302 0 : _gains( 0, i ) = g;
303 :
304 0 : return 0;
305 : }
306 :
307 : template <typename realT>
308 0 : int leakyIntegrator<realT>::gains( realT g )
309 : {
310 0 : for( int i = 0; i < _gains.cols(); ++i )
311 : {
312 0 : _gains( 0, i ) = g;
313 : }
314 :
315 0 : return 0;
316 : }
317 :
318 : template <typename realT>
319 0 : int leakyIntegrator<realT>::gains( const std::vector<realT> &gains )
320 : {
321 :
322 0 : if( gains.size() != _gains.cols() )
323 : {
324 0 : mxError( "leakyIntegrator::gain", MXE_SIZEERR, "input gain vector not same size as number of modes" );
325 0 : return -1; ///\retval -1 on vector size mismatch
326 : }
327 :
328 0 : for( int i = 0; i < _gains.cols(); ++i )
329 : {
330 0 : _gains( 0, i ) = gains[i];
331 : }
332 :
333 0 : return 0;
334 : }
335 :
336 : template <typename realT>
337 0 : int leakyIntegrator<realT>::gains( const std::string &ogainf )
338 : {
339 0 : std::ifstream fin;
340 0 : fin.open( ogainf );
341 :
342 0 : if( !fin.good() )
343 : {
344 0 : mxError( "leakyIntegrator::gains", MXE_FILEOERR, "could not open gan file" );
345 0 : return -1; /// \retval -1 if file open fails
346 : }
347 :
348 0 : std::string tmpstr;
349 : realT g;
350 0 : for( int i = 0; i < _gains.cols(); ++i )
351 : {
352 0 : fin >> tmpstr;
353 0 : g = mx::ioutils::convertFromString<realT>( tmpstr );
354 0 : gain( i, g );
355 : }
356 :
357 0 : return 0;
358 0 : }
359 :
360 : template <typename realT>
361 1 : int leakyIntegrator<realT>::leak( int i, realT l )
362 : {
363 1 : _leaks( 0, i ) = l;
364 :
365 1 : return 0;
366 : }
367 :
368 : template <typename realT>
369 0 : int leakyIntegrator<realT>::leaks( realT l )
370 : {
371 0 : for( int i = 0; i < _leaks.cols(); ++i )
372 0 : _leaks( 0, i ) = l;
373 :
374 0 : return 0;
375 : }
376 :
377 : template <class realT>
378 0 : int leakyIntegrator<realT>::initMeasurements( commandT &filtAmps, commandT &rawAmps )
379 : {
380 0 : filtAmps.measurement.resize( 1, _nModes );
381 0 : filtAmps.measurement.setZero();
382 :
383 0 : rawAmps.measurement.resize( 1, _nModes );
384 0 : rawAmps.measurement.setZero();
385 :
386 0 : return 0;
387 : }
388 :
389 : template <class realT>
390 0 : int leakyIntegrator<realT>::filterCommands( commandT &filtAmps, commandT &rawAmps, int iterNo )
391 : {
392 0 : filtAmps.iterNo = rawAmps.iterNo;
393 :
394 0 : if( _openLoop )
395 : {
396 0 : filtAmps.measurement.setZero();
397 0 : return 0;
398 : }
399 :
400 0 : int topN = rawAmps.measurement.cols();
401 :
402 0 : realT gainF = 1.0;
403 0 : realT HOgainF = 0.0;
404 :
405 : // leaks(0.01);
406 :
407 0 : if( iterNo < _closingDelay )
408 0 : gainF = ( (realT)iterNo ) / _closingDelay;
409 :
410 : // if( iterNo < 0.55*_closingDelay) leaks(0.1);
411 :
412 0 : if( _lowOrders > 0 )
413 : {
414 0 : if( iterNo >= _closingDelay + _lowOrderDelay )
415 : {
416 0 : _lowOrders = 0;
417 : }
418 : else
419 : {
420 0 : topN = _lowOrders;
421 :
422 0 : if( iterNo >= _closingDelay )
423 : {
424 0 : HOgainF = ( (realT)( iterNo - _closingDelay ) ) / ( _lowOrderDelay );
425 : }
426 : }
427 : }
428 :
429 0 : for( int i = 0; i < topN; ++i )
430 : {
431 0 : if( std::isnan( rawAmps.measurement( 0, i ) ) || !std::isfinite( rawAmps.measurement( 0, i ) ) )
432 0 : rawAmps.measurement( 0, i ) = 0.0;
433 :
434 0 : _commands( 0, i ) =
435 0 : ( 1.0 - _leaks( 0, i ) ) * _commands( 0, i ) + gainF * _gains( 0, i ) * rawAmps.measurement( 0, i );
436 :
437 0 : filtAmps.measurement( 0, i ) = _commands( 0, i );
438 : }
439 :
440 0 : for( int i = topN; i < rawAmps.measurement.cols(); ++i )
441 : {
442 0 : if( std::isnan( rawAmps.measurement( 0, i ) ) || !std::isfinite( rawAmps.measurement( 0, i ) ) )
443 0 : rawAmps.measurement( 0, i ) = 0.0;
444 :
445 0 : _commands( 0, i ) =
446 0 : ( 1.0 - _leaks( 0, i ) ) * _commands( 0, i ) + HOgainF * _gains( 0, i ) * rawAmps.measurement( 0, i );
447 :
448 0 : filtAmps.measurement( 0, i ) = _commands( 0, i );
449 :
450 : // filtAmps.measurement(0,i) = 0;
451 : }
452 :
453 0 : return 0;
454 : }
455 :
456 : } // namespace sim
457 : } // namespace AO
458 : } // namespace mx
459 :
460 : #endif //__leakyIntegrator_hpp__
|
