Line data Source code
1 : /** \file clGainOpt.hpp
2 : * \author Jared R. Males (jaredmales@gmail.com)
3 : * \brief Provides a class to manage closed loop gain optimization.
4 : * \ingroup mxAO_files
5 : *
6 : */
7 :
8 : //***********************************************************************//
9 : // Copyright 2016-2020 Jared R. Males (jaredmales@gmail.com)
10 : //
11 : // This file is part of mxlib.
12 : //
13 : // mxlib is free software: you can redistribute it and/or modify
14 : // it under the terms of the GNU General Public License as published by
15 : // the Free Software Foundation, either version 3 of the License, or
16 : // (at your option) any later version.
17 : //
18 : // mxlib is distributed in the hope that it will be useful,
19 : // but WITHOUT ANY WARRANTY; without even the implied warranty of
20 : // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 : // GNU General Public License for more details.
22 : //
23 : // You should have received a copy of the GNU General Public License
24 : // along with mxlib. If not, see <http://www.gnu.org/licenses/>.
25 : //***********************************************************************//
26 :
27 : #ifndef clGainOpt_hpp
28 : #define clGainOpt_hpp
29 :
30 : #ifdef MX_INCLUDE_BOOST
31 : #include <boost/math/tools/minima.hpp>
32 : #endif
33 :
34 : #include <algorithm>
35 : #include <cmath>
36 : #include <limits>
37 : #include <type_traits>
38 :
39 : #include <Eigen/Dense>
40 :
41 : #include "../../sys/timeUtils.hpp"
42 :
43 : #include "../../math/constants.hpp"
44 : #include "../../math/floatUtils.hpp"
45 : #include <error/error_t.hpp>
46 :
47 : // #define ALLOW_F_ZERO
48 :
49 : namespace mx
50 : {
51 : namespace AO
52 : {
53 : namespace analysis
54 : {
55 :
56 : // forward declaration of worker functor
57 : template <typename realT>
58 : struct clGainOptOptGain_OL;
59 :
60 : /// A class to manage optimizing closed-loop gains
61 : /**
62 : * \tparam _realT the real floating point type in which to do all arithmetic. Is used to define the complex type as
63 : * well.
64 : *
65 : * \ingroup mxAOAnalytic
66 : */
67 : template <typename _realT>
68 : struct clGainOpt
69 : {
70 : typedef _realT realT; ///< The real data type
71 : typedef std::complex<_realT> complexT; ///< The complex data type
72 :
73 : /// Termination state of a maximum-stable-gain search.
74 : enum class maxStableGainStatus
75 : {
76 : notRun, ///< No search has been attempted.
77 : crossingFound, ///< A qualifying Nyquist crossing was found.
78 : invalidInput, ///< The frequency grid or derived Nyquist values were invalid.
79 : noCrossing ///< No qualifying Nyquist crossing was found.
80 : };
81 :
82 : /// Diagnostic summary of a maximum-stable-gain search.
83 : struct maxStableGainReport
84 : {
85 : maxStableGainStatus status{ maxStableGainStatus::notRun }; ///< Search termination state.
86 : size_t lowerIndex{ std::numeric_limits<size_t>::max() }; ///< Index below the selected crossing.
87 : size_t upperIndex{ std::numeric_limits<size_t>::max() }; ///< Index above the selected crossing.
88 : realT lowerFrequency{ std::numeric_limits<realT>::quiet_NaN() }; ///< Frequency below the crossing.
89 : realT upperFrequency{ std::numeric_limits<realT>::quiet_NaN() }; ///< Frequency above the crossing.
90 : realT crossingFrequency{ std::numeric_limits<realT>::quiet_NaN() }; ///< Interpolated crossing frequency.
91 : realT crossingReal{ std::numeric_limits<realT>::quiet_NaN() }; ///< Interpolated real Nyquist value.
92 : realT gain{ std::numeric_limits<realT>::quiet_NaN() }; ///< Maximum stable gain at the crossing.
93 : };
94 :
95 : /// Termination state of an open-loop optimum-gain search.
96 : enum class optGainStatus
97 : {
98 : notRun, ///< No search has been attempted.
99 : converged, ///< The minimizer converged inside the search interval.
100 : boundaryLimited, ///< The reported minimum lies on a search boundary.
101 : invalidInput, ///< The PSDs, search controls, or requested interval were invalid.
102 : stabilityFailure, ///< The automatic maximum-stable-gain search failed.
103 : iterationLimit, ///< The minimizer exhausted its iteration limit.
104 : calculationFailure ///< The minimizer threw or returned invalid output.
105 : };
106 :
107 : /// Diagnostic summary of an open-loop optimum-gain search.
108 : struct optGainReport
109 : {
110 : optGainStatus status{ optGainStatus::notRun }; ///< Search termination state.
111 : uintmax_t iterations{ 0 }; ///< Minimizer iterations attempted.
112 : size_t evaluations{ 0 }; ///< Objective evaluations performed.
113 : realT requestedMaximumGain{ std::numeric_limits<realT>::quiet_NaN() }; ///< Caller-supplied gain limit.
114 : realT searchMinimumGain{ std::numeric_limits<realT>::quiet_NaN() }; ///< Final minimizer lower bound.
115 : realT searchMaximumGain{ std::numeric_limits<realT>::quiet_NaN() }; ///< Final minimizer upper bound.
116 : realT minimumEvaluatedGain{ std::numeric_limits<realT>::quiet_NaN() }; ///< Smallest evaluated gain.
117 : realT maximumEvaluatedGain{ std::numeric_limits<realT>::quiet_NaN() }; ///< Largest evaluated gain.
118 : realT gain{ std::numeric_limits<realT>::quiet_NaN() }; ///< Best gain returned by the minimizer.
119 : realT variance{ std::numeric_limits<realT>::quiet_NaN() }; ///< Variance at the best gain.
120 : maxStableGainReport stability; ///< Automatic stability-search diagnostics, when requested.
121 : };
122 :
123 : protected:
124 : int m_N; ///< Number of integrations in the (optional) moving average. Default is 1.
125 : realT m_Ti; ///< The loop sampling interval
126 : realT m_tau; ///< The loop delay
127 :
128 : realT m_remember{ 1.0 }; ///< The leaky integrator forget factor
129 : std::vector<realT> m_b; ///< Vector of FIR coefficients
130 : std::vector<realT> m_a; ///< Vector of IIR coefficients
131 :
132 : std::vector<realT> m_f; ///< Vector of frequencies
133 :
134 : /// True when frequency, sampling interval, or required controller tap count invalidates m_cs and m_ss.
135 : bool m_trigCacheChanged{ true };
136 :
137 : bool m_changed{ true }; ///< True if any of the members which make up the basic transfer functions are changed
138 :
139 : Eigen::Array<realT, -1, -1> m_cs;
140 : Eigen::Array<realT, -1, -1> m_ss;
141 :
142 : std::vector<std::complex<realT>> m_H_dm;
143 : std::vector<std::complex<realT>> m_H_wfs;
144 : std::vector<std::complex<realT>> m_H_ma;
145 : std::vector<std::complex<realT>> m_H_del;
146 : std::vector<std::complex<realT>> m_H_con;
147 :
148 : public:
149 : /** Parameters for stability analysis
150 : * @{
151 : */
152 :
153 : realT m_maxFindMin; ///< The Minimum value for the maximum stable gain finding algorithm.
154 :
155 : ///@}
156 :
157 : /** Parameters for minimization finding
158 : * @{
159 : */
160 :
161 : realT m_minFindMin; ///< The Minimum value for the minimum finding algorithm.
162 : realT m_minFindMaxFact; ///< The maximum value, as a multiplicative factor of maximum gain
163 : int m_minFindBits; ///< The bits of precision to use for minimum finding. Defaults to
164 : ///< std::numeric_limits<realT>::digits.
165 : uintmax_t m_minFindMaxIter; ///< The maximum iterations allowed for minimization.
166 :
167 : ///@}
168 :
169 : /// Default c'tor.
170 : clGainOpt();
171 :
172 : /// C'tor setting the loop timings.
173 : /**
174 : */
175 : clGainOpt( realT Ti, ///< [in] the desired loop sampling interval.
176 : realT tau ///< [in] the desired loop delay.
177 : );
178 :
179 : /// Initialize this instance.
180 : void init();
181 :
182 : /// Get the number of integrations in the (optional) moving average
183 : /**
184 : * \returns the current value of m_N.
185 : */
186 : int N();
187 :
188 : /// Set the number of integrations in the moving average
189 : /**
190 : */
191 : void N( int newN /**< [in] the value of m_N. */ );
192 :
193 : /// Get the loop sampling interval
194 : /**
195 : * \returns the current value of m_Ti.
196 : */
197 : realT Ti();
198 :
199 : /// Set the loop sampling interval
200 : /**
201 : */
202 : void Ti( realT newTi /**< [in] the new value of m_Ti. */ );
203 :
204 : /// Get the loop delay
205 : /**
206 : * \returns the current value of m_tau.
207 : */
208 : realT tau();
209 :
210 : /// Set the loop delay
211 : /**
212 : */
213 : void tau( realT newTau /**< [in] the new value of m_tau.*/ );
214 :
215 : /// Set the vector of FIR coefficients
216 : /**
217 : */
218 : void b( const std::vector<realT> &newB /**< [in] a vector of coefficients, which is copied to m_b.*/ );
219 :
220 : /// Set the vector of FIR coefficients
221 : /**
222 : */
223 : void b( const Eigen::Array<realT, -1, -1>
224 : &newB /**< [in] a column-vector Eigen::Array of coefficients,
225 : which is copied to m_b.*/ );
226 :
227 : /// Get a single FIR coefficient
228 : /**
229 : * \returns a single FIR coefficient
230 : */
231 0 : realT b( size_t i /**< [in] the index of the FIR coefficient*/ )
232 : {
233 0 : return m_b[i];
234 : }
235 :
236 : /// Get the vector of FIR coefficients
237 : /**
238 : */
239 0 : const std::vector<realT> &b()
240 : {
241 0 : return m_b;
242 : }
243 :
244 : void bScale( realT scale );
245 :
246 : /// Set the vector of IIR coefficients
247 : /**
248 : */
249 : void a( const std::vector<realT> &newA /**< [in] a vector of coefficients, which is copied to m_a. */ );
250 :
251 : /// Set the vector of IIR coefficients
252 : /**
253 : */
254 : void a( const Eigen::Array<realT, -1, -1> &newA /**< [in] a column-vector Eigen::Array of
255 : coefficients, which is copied to m_a.*/ );
256 : /// Get a single IIR coefficient
257 : /**
258 : * \returns a single IIR coefficient
259 : */
260 0 : realT a( size_t i )
261 : {
262 0 : return m_a[i];
263 : }
264 :
265 : /// Get the vector of IIR coefficients
266 : /**
267 : */
268 0 : const std::vector<realT> &a()
269 : {
270 0 : return m_a;
271 : }
272 :
273 : void aScale( realT scale );
274 :
275 : /// Set the remember factor for a leaky integrator
276 : void remember( const realT &rem );
277 :
278 : /// Get the remember factor
279 : realT remember();
280 :
281 : /// Set the FIR and IIR coefficients so that the control law is a leaky integrator.
282 : /** Set remember to 1.0 for a pure integrator control law.
283 : *
284 : */
285 : void setLeakyIntegrator( realT remember /**< [in] a number usually close to 1 setting the amount "remembered"
286 : from previous iterations.*/);
287 :
288 : /// Set the vector of frequencies
289 : /**
290 : */
291 : void f( realT *newF, ///< [in] a pointer to an array containing the new frequencies
292 : size_t nF ///< [in] the number of elements of size(realT) in newF.
293 : );
294 :
295 : /// Set the vector of frequencies
296 : /**
297 : */
298 : void f( const std::vector<realT> &newF /**< [in] a vector containing the new frequencies */ );
299 :
300 : /// Get the size of the frequency vector
301 : /**
302 : * \returns m_f.size()
303 : */
304 0 : size_t f_size()
305 : {
306 0 : return m_f.size();
307 : }
308 :
309 : /// Get the i-th value of frequency.
310 : /** No range checks are conducted.
311 : *
312 : * \returns the value of m_f[i]
313 : *
314 : */
315 : realT f( size_t i /**< [in] the index of the frequency to return*/ );
316 :
317 : /// Calculate the open-loop transfer function
318 : /**
319 : * \return the complex value of the open-loop transfer function at f.
320 : */
321 : complexT olXfer( int fi, ///< [in] the index of the frequency
322 : complexT &H_dm, ///< [out] the transfer function of the DM
323 : complexT &H_del, ///< [out] the delay transfer function
324 : complexT &H_con ///< [out] the controller transfer function.
325 : );
326 :
327 : /// Calculate the open-loop transfer function
328 : /**
329 : * \overload
330 : *
331 : * \returns the complex value of the open-loop transfer function at f[fi].
332 : */
333 : complexT olXfer( int fi /**< [in] the index of the frequency */ );
334 :
335 : /// Return the closed loop error transfer function (ETF) at frequency f for gain g.
336 : /**
337 : * \returns the closed loop ETF at f and g.
338 : */
339 : complexT clETF( int fi, ///< [in] the index of the frequency at which to calculate the ETF
340 : realT g ///< [in] the loop gain.
341 : );
342 :
343 : /// Return the closed loop error transfer function (ETF) phase at frequency f for gain g.
344 : /**
345 : * \returns the phase of the closed loop ETF at f and g.
346 : */
347 : realT clETFPhase( int fi, ///< [in] the index of the frequency at which to calculate the ETF
348 : realT g ///< [in] the loop gain.
349 : );
350 :
351 : /// Return the norm of the closed loop error transfer function (ETF) at frequency f for gain g.
352 : /**
353 : * \returns the norm of the closed loop ETF at f and g.
354 : */
355 : realT clETF2( int fi, ///< [in] the index of the frequency at which to calculate the ETF.
356 : realT g ///< [in] the loop gain.
357 : );
358 :
359 : /// Return the closed loop noise transfer function (NTF) at frequency f for gain g.
360 : /**
361 : * \returns the closed loop NTF at f and g.
362 : */
363 : complexT clNTF( int fi, ///< [in] the index of the frequency at which to calculate the NTF
364 : realT g ///< [in] the loop gain.
365 : );
366 :
367 : /// Return the norm of the closed loop noise transfer function (NTF) at frequency f for gain g.
368 : /**
369 : * \returns the value of the closed loop NTF at f and g.
370 : */
371 : realT clNTF2( int fi, ///< [in] the index of the frequency at which to calculate the NTF
372 : realT g ///< [in] the loop gain.
373 : );
374 :
375 : /// Return the norm of the closed loop transfer functions at frequency f for gain g.
376 : /** Calculates both the error transfer function (ETF) and the noise transfer function (NTF).
377 : * This minimizes the various complex number operations, compared to calling both clETF2 and clNTF2.
378 : *
379 : */
380 : void clTF2( realT &ETF, ///< [out] is set to the ETF at f and g
381 : realT &NTF, ///< [out] is set to the NTF at f and g
382 : int fi, ///< [in] is the index of the frequency at which to calculate the ETF
383 : realT g ///< [in] is the loop gain.
384 : );
385 :
386 : /// Calculate the closed loop variance given open-loop PSDs and gain
387 : /** Calculates the following quantities.
388 : \f[
389 : \sigma_{err}^2 = \sum_i \left| ETF(f_i) \right|^2 PSD_{err}(fi) \Delta f\\
390 : \sigma_{noise}^2 = \sum_i \left| NTF(f_i) \right|^2 PSD_{noise}(fi) \Delta f\\
391 : \sigma^2 = \sigma_{err}^2 + \sigma_{noise}^2
392 : \f]
393 : * \f$ \sigma^2 \f$ is returned, and \f$ \sigma_{err}^2 \f$ and \f$ \sigma_{noise}^2 \f$ are available as the
394 : optional
395 : * arguments varErr and varNoise.
396 : *
397 : * \returns the total variance (error + noise) in closed loop
398 : */
399 : realT clVariance( realT &varErr, ///< [out] the variance in the residual process error.
400 : realT &varNoise, ///< [out] the variance in the residual measurement noise.
401 : const std::vector<realT> &PSDerr, ///< [in] the open-loop process error PSD.
402 : const std::vector<realT> &PSDnoise, ///< [in] the open-loop measurement noise PSD.
403 : realT g ///< [in] the gain.
404 : );
405 :
406 : /// Calculate the closed loop variance given open-loop PSDs and gain
407 : /** Overload of clVariance without the varErr and varNoise output parameters.
408 : *
409 : * \overload
410 : *
411 : * \returns the total variance (error + noise) in closed loop
412 : */
413 : realT clVariance( const std::vector<realT> &PSDerr, ///< [in] the open-loop process error PSD.
414 : const std::vector<realT> &PSDnoise, ///< [in] the open-loop measurement noise PSD.
415 : realT g ///< [in] the gain.
416 : );
417 :
418 : /// Find the maximum stable gain for the loop parameters
419 : /** Conducts a search along the Nyquist contour of the open-loop transfer function to find
420 : * the most-negative crossing of the real axis.
421 : *
422 : * Crossings below m_maxFindMin are ignored.
423 : *
424 : * \returns `error_t::noerror` when a crossing is found, `error_t::notfound` when none is found, or an input error.
425 : */
426 : mx::error_t maxStableGain( realT &gain, /**< [out] maximum stable gain; NaN on failure */
427 : maxStableGainReport *report = nullptr /**< [out] optional search diagnostics */ );
428 :
429 : /// Return the optimum closed loop gain given an open loop PSD
430 : /** Determines the maximum stable gain before minimizing the variance.
431 : * \returns `error_t::noerror` on convergence or a boundary-limited result, otherwise an explicit failure status.
432 : */
433 : mx::error_t optGainOpenLoop( realT &gain, ///< [out] optimum gain; NaN on failure
434 : realT &var, ///< [out] variance at the optimum gain; NaN on failure
435 : const std::vector<realT> &PSDerr, ///< [in] open-loop error PSD
436 : const std::vector<realT> &PSDnoise, ///< [in] open-loop measurement-noise PSD
437 : bool gridSearch, ///< [in] whether to perform a coarse initial search
438 : optGainReport *report = nullptr ///< [out] optional search diagnostics
439 : );
440 :
441 : /// Return the optimum closed loop gain given an open loop PSD
442 : /**
443 : * \returns `error_t::noerror` on convergence or a boundary-limited result, otherwise an explicit failure status.
444 : */
445 : mx::error_t optGainOpenLoop( realT &gain, ///< [out] optimum gain; best estimate on timeout
446 : realT &var, ///< [out] variance at the optimum gain
447 : const std::vector<realT> &PSDerr, ///< [in] open-loop error PSD
448 : const std::vector<realT> &PSDnoise, ///< [in] open-loop measurement-noise PSD
449 : realT maximumGain, ///< [in] maximum stable gain bounding the search
450 : bool gridSearch, ///< [in] whether to perform a coarse initial search
451 : optGainReport *report = nullptr ///< [out] optional search diagnostics
452 : );
453 :
454 : /// Calculate the pseudo open-loop PSD given a closed loop PSD
455 : /**
456 : * \returns 0 on success
457 : */
458 : int pseudoOpenLoop( std::vector<realT> &PSD, /**< [in.out] input closed loop PSD, on output contains the pseudo open
459 : loop error PSD */
460 : realT g ///< [in] the loop gain when PSD was measured.
461 : );
462 :
463 : int nyquist( std::vector<realT> &re, std::vector<realT> &im, realT g );
464 : };
465 :
466 : template <typename realT>
467 0 : clGainOpt<realT>::clGainOpt()
468 : {
469 0 : init();
470 0 : }
471 :
472 : template <typename realT>
473 14 : clGainOpt<realT>::clGainOpt( realT Ti, realT tau )
474 : {
475 14 : init();
476 :
477 14 : m_Ti = Ti;
478 14 : m_tau = tau;
479 14 : }
480 :
481 : template <typename realT>
482 14 : void clGainOpt<realT>::init()
483 : {
484 14 : m_N = 1;
485 :
486 14 : setLeakyIntegrator( 1.0 );
487 :
488 14 : m_Ti = 1. / 1000.;
489 14 : m_tau = 2.5 * m_Ti;
490 :
491 14 : m_maxFindMin = 0.0;
492 :
493 14 : m_minFindMin = 1e-9;
494 14 : m_minFindMaxFact = 0.999;
495 14 : m_minFindBits = std::numeric_limits<realT>::digits;
496 14 : m_minFindMaxIter = 10000;
497 :
498 14 : m_trigCacheChanged = true;
499 14 : m_changed = true;
500 14 : }
501 :
502 : template <typename realT>
503 0 : int clGainOpt<realT>::N()
504 : {
505 0 : return m_N;
506 : }
507 :
508 : template <typename realT>
509 0 : void clGainOpt<realT>::N( int newN )
510 : {
511 0 : if( m_N == newN )
512 : {
513 0 : return;
514 : }
515 :
516 0 : m_N = newN;
517 0 : m_changed = true;
518 : }
519 :
520 : template <typename realT>
521 0 : realT clGainOpt<realT>::Ti()
522 : {
523 0 : return m_Ti;
524 : }
525 :
526 : template <typename realT>
527 1 : void clGainOpt<realT>::Ti( realT newTi )
528 : {
529 1 : if( m_Ti == newTi )
530 : {
531 0 : return;
532 : }
533 :
534 1 : m_Ti = newTi;
535 1 : m_trigCacheChanged = true;
536 1 : m_changed = true;
537 : }
538 :
539 : template <typename realT>
540 0 : realT clGainOpt<realT>::tau()
541 : {
542 0 : return m_tau;
543 : }
544 :
545 : template <typename realT>
546 0 : void clGainOpt<realT>::tau( realT newTau )
547 : {
548 0 : if( m_tau == newTau )
549 : {
550 0 : return;
551 : }
552 :
553 0 : m_tau = newTau;
554 0 : m_changed = true;
555 : }
556 :
557 : template <typename realT>
558 123 : void clGainOpt<realT>::b( const std::vector<realT> &newB )
559 : {
560 123 : if( newB.size() > (size_t)m_cs.cols() )
561 : {
562 5 : m_trigCacheChanged = true;
563 : }
564 :
565 123 : m_b = newB;
566 123 : m_changed = true;
567 123 : }
568 :
569 : template <typename realT>
570 0 : void clGainOpt<realT>::b( const Eigen::Array<realT, -1, -1> &newB )
571 : {
572 0 : if( newB.cols() > m_cs.cols() )
573 : {
574 0 : m_trigCacheChanged = true;
575 : }
576 :
577 0 : m_b.resize( newB.cols() );
578 :
579 0 : for( size_t i = 0; i < m_b.size(); ++i )
580 : {
581 0 : m_b[i] = newB( 0, i );
582 : }
583 :
584 0 : m_changed = true;
585 0 : }
586 :
587 : template <typename realT>
588 0 : void clGainOpt<realT>::bScale( realT scale )
589 : {
590 0 : for( size_t n = 0; n < m_b.size(); ++n )
591 : {
592 0 : m_b[n] *= scale;
593 : }
594 :
595 0 : m_changed = true;
596 0 : }
597 :
598 : template <typename realT>
599 123 : void clGainOpt<realT>::a( const std::vector<realT> &newA )
600 : {
601 123 : if( newA.size() + 1 > (size_t)m_cs.cols() )
602 : {
603 5 : m_trigCacheChanged = true;
604 : }
605 :
606 123 : m_a = newA;
607 123 : m_changed = true;
608 123 : }
609 :
610 : template <typename realT>
611 0 : void clGainOpt<realT>::a( const Eigen::Array<realT, -1, -1> &newA )
612 : {
613 0 : if( newA.cols() + 1 > m_cs.cols() )
614 : {
615 0 : m_trigCacheChanged = true;
616 : }
617 :
618 0 : m_a.resize( newA.cols() );
619 :
620 0 : for( size_t i = 0; i < m_a.size(); ++i )
621 : {
622 0 : m_a[i] = newA( 0, i );
623 : }
624 :
625 0 : m_changed = true;
626 0 : }
627 :
628 : template <typename realT>
629 0 : void clGainOpt<realT>::aScale( realT scale )
630 : {
631 0 : for( size_t n = 0; n < m_a.size(); ++n )
632 : {
633 0 : m_a[n] *= scale;
634 : }
635 :
636 0 : m_changed = true;
637 0 : }
638 :
639 : template <typename realT>
640 2 : void clGainOpt<realT>::remember( const realT &rem )
641 : {
642 2 : if( m_remember != rem )
643 : {
644 2 : m_remember = rem;
645 :
646 2 : m_changed = true;
647 : }
648 2 : }
649 :
650 : template <typename realT>
651 0 : realT clGainOpt<realT>::remember()
652 : {
653 0 : return m_remember;
654 : }
655 :
656 : template <typename realT>
657 14 : void clGainOpt<realT>::setLeakyIntegrator( realT remember )
658 : {
659 14 : if( m_b.size() != 1 || m_a.size() != 1 || m_b[0] != 1.0 || m_a[0] != 1.0 || m_remember != remember )
660 : {
661 14 : if( m_b.size() != 1 )
662 : {
663 14 : m_b.resize( 1 );
664 14 : m_trigCacheChanged = true;
665 : }
666 :
667 14 : m_b[0] = 1.0;
668 :
669 14 : if( m_a.size() != 1 )
670 : {
671 14 : m_a.resize( 1 );
672 14 : m_trigCacheChanged = true;
673 : }
674 :
675 14 : m_a[0] = 1.0;
676 :
677 14 : m_remember = remember;
678 :
679 14 : m_changed = true;
680 : }
681 14 : }
682 :
683 : template <typename realT>
684 0 : void clGainOpt<realT>::f( realT *newF, size_t nF )
685 : {
686 0 : m_f.resize( nF );
687 0 : for( int i = 0; i < nF; ++i )
688 : {
689 0 : m_f[i] = newF[i];
690 : }
691 :
692 0 : m_trigCacheChanged = true;
693 0 : m_changed = true;
694 0 : }
695 :
696 : template <typename realT>
697 17 : void clGainOpt<realT>::f( const std::vector<realT> &newF )
698 : {
699 17 : m_f = newF;
700 17 : m_trigCacheChanged = true;
701 :
702 17 : m_changed = true;
703 17 : }
704 :
705 : template <typename realT>
706 0 : realT clGainOpt<realT>::f( size_t i )
707 : {
708 :
709 0 : return m_f[i];
710 : }
711 :
712 : template <typename realT>
713 7783 : std::complex<realT> clGainOpt<realT>::olXfer( int fi )
714 : {
715 7783 : complexT H_dm;
716 7783 : complexT H_del;
717 7783 : complexT H_con;
718 :
719 15566 : return olXfer( fi, H_dm, H_del, H_con );
720 : }
721 :
722 : // If PRECALC_TRIG is defined, then the cosine and sine tables are pre-calculated and used instead of repeated exp(-i)
723 : // calls. This is much much faster, though uses more memory. In general, only undefine this for testing or debugging.
724 : #define PRECALC_TRIG
725 :
726 : template <typename realT>
727 122835 : std::complex<realT> clGainOpt<realT>::olXfer( int fi, complexT &H_dm, complexT &H_del, complexT &H_con )
728 : {
729 : // clang-format off
730 : #ifndef ALLOW_F_ZERO
731 122835 : if( m_f[fi] <= 0 )
732 : {
733 : #else
734 : if( m_f[fi] < 0 )
735 : {
736 : #endif // clang-format on
737 :
738 0 : H_dm = 0;
739 0 : H_del = 0;
740 0 : H_con = 0;
741 0 : return 0;
742 : }
743 :
744 : #ifdef PRECALC_TRIG
745 122835 : if( m_trigCacheChanged )
746 : {
747 11 : size_t jmax = std::max( m_a.size() + 1, m_b.size() );
748 :
749 11 : m_cs.resize( m_f.size(), jmax );
750 11 : m_ss.resize( m_f.size(), jmax );
751 :
752 1225 : for( size_t i = 0; i < m_f.size(); ++i )
753 : {
754 1214 : m_cs( i, 0 ) = 1.0;
755 1214 : m_ss( i, 0 ) = 0.0;
756 :
757 3028 : for( size_t j = 1; j < jmax; ++j )
758 : {
759 1814 : m_cs( i, j ) = cos( math::two_pi<realT>() * m_f[i] * m_Ti * realT( j ) );
760 1814 : m_ss( i, j ) = sin( math::two_pi<realT>() * m_f[i] * m_Ti * realT( j ) );
761 : }
762 : }
763 :
764 11 : m_trigCacheChanged = false;
765 : }
766 : #endif
767 :
768 122835 : if( m_changed )
769 : {
770 129 : m_H_dm.resize( m_f.size(), 0 );
771 129 : m_H_wfs.resize( m_f.size(), 0 );
772 129 : m_H_ma.resize( m_f.size(), 0 );
773 129 : m_H_del.resize( m_f.size(), 0 );
774 129 : m_H_con.resize( m_f.size(), 0 );
775 :
776 129 : size_t jmax = std::min( m_a.size(), m_b.size() );
777 :
778 : // #pragma omp parallel for
779 8895 : for( size_t i = 0; i < m_f.size(); ++i )
780 : {
781 : // clang-format off
782 : #ifndef ALLOW_F_ZERO
783 8766 : if( m_f[i] <= 0 )
784 : {
785 0 : continue;
786 : }
787 : #else
788 : if( m_f[i] < 0 )
789 : {
790 : continue;
791 : }
792 : #endif // clang-format on
793 :
794 8766 : complexT s = complexT( 0.0, math::two_pi<realT>() * m_f[i] );
795 :
796 8766 : complexT expsT = exp( -s * m_Ti );
797 :
798 8766 : if( m_f[i] == 0 )
799 : {
800 0 : m_H_dm[i] = std::complex<realT>( 1, 0 );
801 : }
802 : else
803 : {
804 8766 : m_H_dm[i] = ( realT( 1 ) - expsT ) / ( s * m_Ti );
805 : }
806 :
807 8766 : m_H_wfs[i] = m_H_dm[i];
808 :
809 8766 : m_H_ma[i] = 1; // realT(1./m_N)*(realT(1) - pow(expsT,m_N))/(realT(1) - expsT);
810 :
811 8766 : m_H_del[i] = exp( -s * m_tau );
812 :
813 8766 : complexT FIR = complexT( m_b[0], 0 );
814 :
815 8766 : complexT IIR = complexT( 0.0, 0.0 );
816 32022 : for( size_t j = 1; j < jmax; ++j )
817 : {
818 : #ifdef PRECALC_TRIG
819 23256 : realT cs = m_cs( i, j );
820 23256 : realT ss = m_ss( i, j );
821 23256 : FIR += m_b[j] * complexT( cs, -ss );
822 23256 : IIR += m_remember * m_a[j - 1] * complexT( cs, -ss );
823 : #else
824 : complexT expZ = exp( -s * m_Ti * realT( j ) );
825 : FIR += m_b[j] * expZ;
826 : IIR += m_remember * m_a[j - 1] * expZ;
827 : #endif
828 : }
829 :
830 17532 : for( size_t jj = jmax; jj < m_a.size() + 1; ++jj )
831 : {
832 : // clang-format off
833 : #ifdef PRECALC_TRIG
834 8766 : realT cs = m_cs( i, jj );
835 8766 : realT ss = m_ss( i, jj );
836 8766 : IIR += m_remember * m_a[jj - 1] * complexT( cs, -ss );
837 : #else
838 : complexT expZ = exp( -s * m_Ti * realT( jj ) );
839 : IIR += m_remember * m_a[jj - 1] * expZ;
840 : #endif // clang-format on
841 : }
842 :
843 8766 : for( size_t jj = jmax; jj < m_b.size(); ++jj )
844 : {
845 : #ifdef PRECALC_TRIG
846 0 : realT cs = m_cs( i, jj );
847 0 : realT ss = m_ss( i, jj );
848 0 : FIR += m_b[jj] * complexT( cs, -ss );
849 : #else
850 : complexT expZ = exp( -s * m_Ti * realT( jj ) );
851 : FIR += m_b[jj] * expZ;
852 : #endif
853 : }
854 :
855 8766 : m_H_con[i] = FIR / ( realT( 1.0 ) - IIR );
856 :
857 : /*if( i == 0 || i == 1)
858 : {
859 : std::cerr << i << " " << m_f[fi] << " " << s << " " << expsT << " " << m_H_wfs[i] << " " << m_H_dm[i] <<
860 : " "
861 : << m_H_con[i] << " " << m_H_del[i] << "\n";
862 : //exit(0);
863 : }*/
864 : }
865 :
866 129 : m_changed = false;
867 : }
868 :
869 122835 : H_dm = m_H_dm[fi];
870 122835 : H_del = m_H_del[fi]; //*m_H_ma[fi];
871 122835 : H_con = m_H_con[fi];
872 :
873 122835 : return ( m_H_dm[fi] * m_H_wfs[fi] * m_H_del[fi] * m_H_con[fi] );
874 : }
875 :
876 : template <typename realT>
877 8 : std::complex<realT> clGainOpt<realT>::clETF( int fi, realT g )
878 : {
879 : #ifndef ALLOW_F_ZERO
880 8 : if( m_f[fi] <= 0 )
881 0 : return 0;
882 : #else
883 : if( m_f[fi] < 0 )
884 : return 0;
885 : #endif
886 :
887 8 : return ( realT( 1 ) / ( realT( 1 ) + g * olXfer( fi ) ) );
888 : }
889 :
890 : template <typename realT>
891 8 : realT clGainOpt<realT>::clETFPhase( int fi, realT g )
892 : {
893 : #ifndef ALLOW_F_ZERO
894 8 : if( m_f[fi] <= 0 )
895 0 : return 0;
896 : #else
897 : if( m_f[fi] < 0 )
898 : return 0;
899 : #endif
900 :
901 8 : return std::arg( ( realT( 1 ) / ( realT( 1 ) + g * olXfer( fi ) ) ) );
902 : }
903 :
904 : template <typename realT>
905 8 : realT clGainOpt<realT>::clETF2( int fi, realT g )
906 : {
907 : #ifndef ALLOW_F_ZERO
908 8 : if( m_f[fi] <= 0 )
909 0 : return 0;
910 : #else
911 : if( m_f[fi] < 0 )
912 : return 0;
913 : #endif
914 :
915 8 : return norm( realT( 1 ) / ( realT( 1 ) + g * olXfer( fi ) ) );
916 : }
917 :
918 : template <typename realT>
919 8 : std::complex<realT> clGainOpt<realT>::clNTF( int fi, realT g )
920 : {
921 : #ifndef ALLOW_F_ZERO
922 8 : if( m_f[fi] <= 0 )
923 0 : return 0;
924 : #else
925 : if( m_f[fi] < 0 )
926 : return 0;
927 : #endif
928 :
929 8 : complexT H_dm, H_del, H_con;
930 :
931 8 : complexT olX = olXfer( fi, H_dm, H_del, H_con ); // H_dm*H_wfs*H_ma*H_del*H_con;
932 :
933 8 : return -( H_dm * H_del * g * H_con ) / ( realT( 1 ) + g * olX );
934 : }
935 :
936 : template <typename realT>
937 8 : realT clGainOpt<realT>::clNTF2( int fi, realT g )
938 : {
939 : #ifndef ALLOW_F_ZERO
940 8 : if( m_f[fi] <= 0 )
941 0 : return 0;
942 : #else
943 : if( m_f[fi] < 0 )
944 : return 0;
945 : #endif
946 :
947 8 : complexT H_dm, H_del, H_con;
948 :
949 8 : complexT olX = olXfer( fi, H_dm, H_del, H_con ); // H_dm*H_wfs*H_ma*H_del*H_con;
950 :
951 8 : complexT NTF = -( H_dm * H_del * g * H_con ) / ( realT( 1 ) + g * olX );
952 :
953 8 : return norm( NTF );
954 : }
955 :
956 : template <typename realT>
957 115028 : void clGainOpt<realT>::clTF2( realT &ETF, realT &NTF, int fi, realT g )
958 : {
959 : #ifndef ALLOW_F_ZERO
960 115028 : if( m_f[fi] <= 0 )
961 : #else
962 : if( m_f[fi] < 0 )
963 : #endif
964 : {
965 0 : ETF = 0;
966 0 : NTF = 0;
967 0 : return;
968 : }
969 :
970 115028 : complexT H_dm, H_del, H_con;
971 :
972 115028 : complexT olX = olXfer( fi, H_dm, H_del, H_con ); // H_dm*H_wfs*H_ma*H_del*H_con;
973 :
974 115028 : if( m_f[fi] == 0 )
975 : {
976 : }
977 :
978 115028 : ETF = norm( realT( 1 ) / ( realT( 1 ) + g * olX ) );
979 115028 : NTF = norm( -( H_dm * H_del * g * H_con ) / ( realT( 1 ) + g * olX ) );
980 :
981 : /*if(m_f[fi] == 0)
982 : {
983 : std::cerr << "ETF: " << ETF << " NTF: " << NTF << "\n";
984 : }*/
985 : }
986 :
987 : template <typename realT>
988 1717 : realT clGainOpt<realT>::clVariance(
989 : realT &varErr, realT &varNoise, const std::vector<realT> &PSDerr, const std::vector<realT> &PSDnoise, realT g )
990 : {
991 1717 : if( m_f.size() != PSDerr.size() || m_f.size() != PSDnoise.size() )
992 : {
993 0 : std::cerr << "clVariance: Frequency grid and PSDs must be same size." << std::endl;
994 0 : return -1;
995 : }
996 :
997 : realT ETF, NTF, df;
998 :
999 1717 : varErr = 0;
1000 1717 : varNoise = 0;
1001 :
1002 1717 : df = m_f[1] - m_f[0];
1003 :
1004 116737 : for( size_t i = 0; i < PSDerr.size(); ++i )
1005 : {
1006 115020 : if( g == 0 )
1007 : {
1008 0 : ETF = 1;
1009 0 : NTF = 0;
1010 : }
1011 : else
1012 : {
1013 115020 : clTF2( ETF, NTF, i, g );
1014 : }
1015 115020 : varErr += ETF * PSDerr[i] * df;
1016 115020 : varNoise += NTF * PSDnoise[i] * df;
1017 : }
1018 :
1019 1717 : return varErr + varNoise;
1020 : }
1021 :
1022 : template <typename realT>
1023 1717 : realT clGainOpt<realT>::clVariance( const std::vector<realT> &PSDerr, const std::vector<realT> &PSDnoise, realT g )
1024 : {
1025 : realT varErr;
1026 : realT varNoise;
1027 :
1028 3434 : return clVariance( varErr, varNoise, PSDerr, PSDnoise, g );
1029 : }
1030 :
1031 : template <typename realT>
1032 125 : mx::error_t clGainOpt<realT>::maxStableGain( realT &gain, maxStableGainReport *report )
1033 : {
1034 125 : maxStableGainReport localReport;
1035 125 : maxStableGainReport &activeReport = report == nullptr ? localReport : *report;
1036 125 : activeReport = {};
1037 125 : gain = std::numeric_limits<realT>::quiet_NaN();
1038 :
1039 125 : if( m_f.size() < 2 )
1040 : {
1041 1 : activeReport.status = maxStableGainStatus::invalidInput;
1042 1 : return error_t::sizeerr;
1043 : }
1044 :
1045 7878 : for( size_t index = 0; index < m_f.size(); ++index )
1046 : {
1047 7754 : if( !math::isFinite( m_f[index] ) || m_f[index] < 0 || ( index > 0 && m_f[index] <= m_f[index - 1] ) )
1048 : {
1049 0 : activeReport.status = maxStableGainStatus::invalidInput;
1050 0 : return error_t::invalidarg;
1051 : }
1052 : }
1053 :
1054 124 : std::vector<realT> re, im;
1055 :
1056 124 : nyquist( re, im, 1.0 );
1057 :
1058 7878 : for( size_t index = 0; index < re.size(); ++index )
1059 : {
1060 7754 : if( !math::isFinite( re[index] ) || !math::isFinite( im[index] ) )
1061 : {
1062 0 : activeReport.status = maxStableGainStatus::invalidInput;
1063 0 : return error_t::error;
1064 : }
1065 : }
1066 :
1067 124 : bool crossingFound = false;
1068 7754 : for( size_t index = 0; index + 1 < re.size(); ++index )
1069 : {
1070 7630 : if( !( im[index] < 0 && im[index + 1] >= 0 ) )
1071 : {
1072 7508 : continue;
1073 : }
1074 :
1075 122 : const realT fraction = -im[index] / ( im[index + 1] - im[index] );
1076 122 : const realT crossingReal = re[index] + fraction * ( re[index + 1] - re[index] );
1077 122 : const realT crossingGain = -realT( 1 ) / crossingReal;
1078 122 : if( crossingReal >= 0 || !math::isFinite( crossingGain ) || crossingGain < m_maxFindMin )
1079 : {
1080 0 : continue;
1081 : }
1082 :
1083 122 : if( crossingFound && crossingReal >= activeReport.crossingReal )
1084 : {
1085 0 : continue;
1086 : }
1087 :
1088 122 : crossingFound = true;
1089 122 : activeReport.lowerIndex = index;
1090 122 : activeReport.upperIndex = index + 1;
1091 122 : activeReport.lowerFrequency = m_f[index];
1092 122 : activeReport.upperFrequency = m_f[index + 1];
1093 122 : activeReport.crossingFrequency = m_f[index] + fraction * ( m_f[index + 1] - m_f[index] );
1094 122 : activeReport.crossingReal = crossingReal;
1095 122 : activeReport.gain = crossingGain;
1096 : }
1097 :
1098 124 : if( !crossingFound )
1099 : {
1100 2 : activeReport.status = maxStableGainStatus::noCrossing;
1101 2 : return error_t::notfound;
1102 : }
1103 :
1104 122 : activeReport.status = maxStableGainStatus::crossingFound;
1105 122 : gain = activeReport.gain;
1106 122 : return error_t::noerror;
1107 124 : }
1108 :
1109 : // Implement the minimization, allowing pre-compiled specializations
1110 : namespace impl
1111 : {
1112 :
1113 : template <typename realT>
1114 : /// Minimize an open-loop variance objective on a bounded gain interval.
1115 : mx::error_t optGainOpenLoop( realT &gain, ///< [out] best gain estimate
1116 : realT &var, ///< [out] variance at the best gain estimate
1117 : clGainOptOptGain_OL<realT> &olgo, ///< [in,out] variance objective and diagnostics
1118 : const realT &minimumGain, ///< [in] lower gain bound
1119 : const realT &maximumGain, ///< [in] upper gain bound
1120 : int minFindBits, ///< [in] requested precision in binary digits
1121 : uintmax_t minFindMaxIter, ///< [in] maximum minimizer iterations
1122 : uintmax_t &iters ///< [out] minimizer iterations used
1123 : )
1124 : {
1125 : #ifdef MX_INCLUDE_BOOST
1126 : gain = std::numeric_limits<realT>::quiet_NaN();
1127 : var = std::numeric_limits<realT>::quiet_NaN();
1128 :
1129 : try
1130 : {
1131 : std::pair<realT, realT> brack;
1132 : brack = boost::math::tools::brentm_findm_minima<clGainOptOptGain_OL<realT>, realT>( olgo,
1133 : minimumGain,
1134 : maximumGain,
1135 : minFindBits,
1136 : minFindMaxIter,
1137 : iters );
1138 : gain = brack.first;
1139 : var = brack.second;
1140 : }
1141 : catch( ... )
1142 : {
1143 : return error_t::exception;
1144 : }
1145 :
1146 : if( iters >= minFindMaxIter )
1147 : {
1148 : return error_t::timeout;
1149 : }
1150 :
1151 : return error_t::noerror;
1152 : #else
1153 : static_assert( std::is_fundamental<realT>::value || !std::is_fundamental<realT>::value,
1154 : "impl::optGainOpenLoop<realT> is not specialized for type realT, and MX_INCLUDE_BOOST is not "
1155 : "defined, so I can't just use boost." );
1156 : return error_t::notimpl;
1157 : #endif
1158 : }
1159 :
1160 : template <>
1161 : /// Float specialization of the bounded open-loop gain minimizer.
1162 : mx::error_t optGainOpenLoop<float>( float &gain, ///< [out] best gain estimate
1163 : float &var, ///< [out] variance at the best gain estimate
1164 : clGainOptOptGain_OL<float> &olgo, ///< [in,out] variance objective and diagnostics
1165 : const float &minimumGain, ///< [in] lower gain bound
1166 : const float &maximumGain, ///< [in] upper gain bound
1167 : int minFindBits, ///< [in] requested precision in binary digits
1168 : uintmax_t minFindMaxIter, ///< [in] maximum minimizer iterations
1169 : uintmax_t &iters ///< [out] minimizer iterations used
1170 : );
1171 :
1172 : template <>
1173 : /// Double specialization of the bounded open-loop gain minimizer.
1174 : mx::error_t optGainOpenLoop<double>( double &gain, ///< [out] best gain estimate
1175 : double &var, ///< [out] variance at the best gain estimate
1176 : clGainOptOptGain_OL<double> &olgo, ///< [in,out] variance objective and diagnostics
1177 : const double &minimumGain, ///< [in] lower gain bound
1178 : const double &maximumGain, ///< [in] upper gain bound
1179 : int minFindBits, ///< [in] requested precision in binary digits
1180 : uintmax_t minFindMaxIter, ///< [in] maximum minimizer iterations
1181 : uintmax_t &iters ///< [out] minimizer iterations used
1182 : );
1183 :
1184 : template <>
1185 : /// Long-double specialization of the bounded open-loop gain minimizer.
1186 : mx::error_t
1187 : optGainOpenLoop<long double>( long double &gain, ///< [out] best gain estimate
1188 : long double &var, ///< [out] variance at the best gain estimate
1189 : clGainOptOptGain_OL<long double> &olgo, ///< [in,out] variance objective and diagnostics
1190 : const long double &minimumGain, ///< [in] lower gain bound
1191 : const long double &maximumGain, ///< [in] upper gain bound
1192 : int minFindBits, ///< [in] requested precision in binary digits
1193 : uintmax_t minFindMaxIter, ///< [in] maximum minimizer iterations
1194 : uintmax_t &iters ///< [out] minimizer iterations used
1195 : );
1196 :
1197 : #ifdef HASQUAD
1198 : template <>
1199 : /// Quad-precision specialization of the bounded open-loop gain minimizer.
1200 : mx::error_t
1201 : optGainOpenLoop<_m_float128>( _m_float128 &gain, ///< [out] best gain estimate
1202 : _m_float128 &var, ///< [out] variance at the best gain estimate
1203 : clGainOptOptGain_OL<_m_float128> &olgo, ///< [in,out] variance objective and diagnostics
1204 : const _m_float128 &minimumGain, ///< [in] lower gain bound
1205 : const _m_float128 &maximumGain, ///< [in] upper gain bound
1206 : int minFindBits, ///< [in] requested precision in binary digits
1207 : uintmax_t minFindMaxIter, ///< [in] maximum minimizer iterations
1208 : uintmax_t &iters ///< [out] minimizer iterations used
1209 : );
1210 : #endif
1211 :
1212 : } // namespace impl
1213 :
1214 : template <typename realT>
1215 1 : mx::error_t clGainOpt<realT>::optGainOpenLoop( realT &gain,
1216 : realT &var,
1217 : const std::vector<realT> &PSDerr,
1218 : const std::vector<realT> &PSDnoise,
1219 : bool gridSearch,
1220 : optGainReport *report )
1221 : {
1222 1 : maxStableGainReport stabilityReport;
1223 : realT maximumGain;
1224 1 : error_t rv = maxStableGain( maximumGain, &stabilityReport );
1225 1 : if( rv != error_t::noerror )
1226 : {
1227 1 : gain = std::numeric_limits<realT>::quiet_NaN();
1228 1 : var = std::numeric_limits<realT>::quiet_NaN();
1229 1 : if( report != nullptr )
1230 : {
1231 1 : *report = {};
1232 1 : report->status = optGainStatus::stabilityFailure;
1233 1 : report->stability = stabilityReport;
1234 : }
1235 1 : return rv;
1236 : }
1237 :
1238 0 : optGainReport optimizationReport;
1239 0 : rv = optGainOpenLoop( gain, var, PSDerr, PSDnoise, maximumGain, gridSearch, &optimizationReport );
1240 0 : optimizationReport.stability = stabilityReport;
1241 0 : if( report != nullptr )
1242 : {
1243 0 : *report = optimizationReport;
1244 : }
1245 0 : return rv;
1246 : }
1247 :
1248 : template <typename realT>
1249 126 : mx::error_t clGainOpt<realT>::optGainOpenLoop( realT &gain,
1250 : realT &var,
1251 : const std::vector<realT> &PSDerr,
1252 : const std::vector<realT> &PSDnoise,
1253 : realT maximumGain,
1254 : bool gridSearch,
1255 : optGainReport *report )
1256 : {
1257 126 : optGainReport localReport;
1258 126 : optGainReport &activeReport = report == nullptr ? localReport : *report;
1259 126 : activeReport = {};
1260 126 : activeReport.requestedMaximumGain = maximumGain;
1261 126 : gain = std::numeric_limits<realT>::quiet_NaN();
1262 126 : var = std::numeric_limits<realT>::quiet_NaN();
1263 :
1264 126 : if( m_f.size() < 2 || PSDerr.size() != m_f.size() || PSDnoise.size() != m_f.size() )
1265 : {
1266 0 : activeReport.status = optGainStatus::invalidInput;
1267 0 : return error_t::sizeerr;
1268 : }
1269 :
1270 252 : if( !math::isFinite( maximumGain ) || maximumGain <= 0 || !math::isFinite( m_minFindMin ) || m_minFindMin < 0 ||
1271 378 : !math::isFinite( m_minFindMaxFact ) || m_minFindMaxFact <= 0 || m_minFindMaxFact > 1 || m_minFindBits <= 0 ||
1272 126 : m_minFindMaxIter == 0 )
1273 : {
1274 0 : activeReport.status = optGainStatus::invalidInput;
1275 0 : return error_t::invalidconfig;
1276 : }
1277 :
1278 126 : const realT requestedMinimum = m_minFindMin;
1279 126 : const realT requestedMaximum = m_minFindMaxFact * maximumGain;
1280 126 : if( !math::isFinite( requestedMaximum ) || requestedMaximum <= requestedMinimum )
1281 : {
1282 1 : activeReport.status = optGainStatus::invalidInput;
1283 1 : return error_t::invalidconfig;
1284 : }
1285 :
1286 125 : clGainOptOptGain_OL<realT> olgo;
1287 125 : olgo.go = this;
1288 125 : olgo.PSDerr = &PSDerr;
1289 125 : olgo.PSDnoise = &PSDnoise;
1290 :
1291 125 : realT minimumGain = requestedMinimum;
1292 125 : realT maximumSearchGain = requestedMaximum;
1293 125 : bool searchBoundarySelected = false;
1294 :
1295 125 : if( gridSearch )
1296 : {
1297 1 : const realT gainStep = std::min( realT( 0.05 ), requestedMaximum - requestedMinimum );
1298 1 : realT currentGain = requestedMaximum;
1299 1 : realT minimumVariance = olgo( currentGain );
1300 1 : realT gainAtMinimum = currentGain;
1301 :
1302 2 : while( currentGain > requestedMinimum )
1303 : {
1304 1 : const realT nextGain = std::max( requestedMinimum, currentGain - gainStep );
1305 1 : if( nextGain >= currentGain )
1306 : {
1307 0 : break;
1308 : }
1309 :
1310 1 : currentGain = nextGain;
1311 1 : const realT candidateVariance = olgo( currentGain );
1312 :
1313 1 : if( candidateVariance < minimumVariance )
1314 : {
1315 0 : minimumVariance = candidateVariance;
1316 0 : gainAtMinimum = currentGain;
1317 : }
1318 : }
1319 :
1320 1 : minimumGain = std::max( requestedMinimum, gainAtMinimum - gainStep );
1321 1 : maximumSearchGain = std::min( requestedMaximum, gainAtMinimum + gainStep );
1322 1 : searchBoundarySelected = gainAtMinimum == requestedMinimum || gainAtMinimum == requestedMaximum;
1323 : }
1324 :
1325 125 : activeReport.searchMinimumGain = minimumGain;
1326 125 : activeReport.searchMaximumGain = maximumSearchGain;
1327 :
1328 125 : uintmax_t iterations = m_minFindMaxIter;
1329 125 : error_t rv = impl::optGainOpenLoop( gain,
1330 : var,
1331 : olgo,
1332 : minimumGain,
1333 : maximumSearchGain,
1334 : m_minFindBits,
1335 : m_minFindMaxIter,
1336 : iterations );
1337 :
1338 125 : activeReport.iterations = iterations;
1339 125 : activeReport.evaluations = olgo.evaluations;
1340 125 : activeReport.minimumEvaluatedGain = olgo.minimumEvaluatedGain;
1341 125 : activeReport.maximumEvaluatedGain = olgo.maximumEvaluatedGain;
1342 125 : activeReport.gain = gain;
1343 125 : activeReport.variance = var;
1344 :
1345 125 : if( rv == error_t::timeout )
1346 : {
1347 1 : activeReport.status = optGainStatus::iterationLimit;
1348 1 : return rv;
1349 : }
1350 :
1351 124 : if( rv != error_t::noerror || !math::isFinite( gain ) || !math::isFinite( var ) )
1352 : {
1353 0 : activeReport.status = optGainStatus::calculationFailure;
1354 0 : gain = std::numeric_limits<realT>::quiet_NaN();
1355 0 : var = std::numeric_limits<realT>::quiet_NaN();
1356 0 : activeReport.gain = gain;
1357 0 : activeReport.variance = var;
1358 0 : return rv == error_t::noerror ? error_t::error : rv;
1359 : }
1360 :
1361 124 : if( searchBoundarySelected || gain <= minimumGain || gain >= maximumSearchGain )
1362 : {
1363 3 : activeReport.status = optGainStatus::boundaryLimited;
1364 : }
1365 : else
1366 : {
1367 121 : activeReport.status = optGainStatus::converged;
1368 : }
1369 :
1370 124 : return error_t::noerror;
1371 : }
1372 :
1373 : template <typename realT>
1374 0 : int clGainOpt<realT>::pseudoOpenLoop( std::vector<realT> &PSD, realT g )
1375 : {
1376 : realT e;
1377 0 : for( int f = 0; f < m_f.size(); ++f )
1378 : {
1379 0 : e = clETF2( f, g );
1380 :
1381 0 : if( e > 0 )
1382 0 : PSD[f] = PSD[f] / e;
1383 : }
1384 :
1385 0 : return 0;
1386 : }
1387 :
1388 : template <typename realT>
1389 124 : int clGainOpt<realT>::nyquist( std::vector<realT> &re, std::vector<realT> &im, realT g )
1390 : {
1391 124 : re.resize( m_f.size() );
1392 124 : im.resize( m_f.size() );
1393 :
1394 124 : complexT etf;
1395 :
1396 7878 : for( size_t f = 0; f < m_f.size(); ++f )
1397 : {
1398 7754 : etf = g * olXfer( f ); // clETF(f, g);
1399 7754 : re[f] = real( etf );
1400 7754 : im[f] = imag( etf );
1401 : }
1402 :
1403 124 : return 0;
1404 : }
1405 :
1406 : //------------ Workers ---------------------
1407 :
1408 : /// Bisection worker struct for finding optimum closed loop gain from open loop PSDs
1409 : template <typename realT>
1410 : struct clGainOptOptGain_OL
1411 : {
1412 : clGainOpt<realT> *go{ nullptr }; ///< Gain optimizer used to evaluate variance.
1413 : const std::vector<realT> *PSDerr{ nullptr }; ///< Open-loop disturbance PSD.
1414 : const std::vector<realT> *PSDnoise{ nullptr }; ///< Measurement-noise PSD.
1415 : size_t evaluations{ 0 }; ///< Objective evaluations performed.
1416 : realT minimumEvaluatedGain{ std::numeric_limits<realT>::max() }; ///< Smallest gain evaluated.
1417 : realT maximumEvaluatedGain{ std::numeric_limits<realT>::lowest() }; ///< Largest gain evaluated.
1418 :
1419 : /// Evaluate closed-loop variance at a candidate gain and update diagnostics.
1420 1715 : realT operator()( const realT &g /**< [in] candidate gain */ )
1421 : {
1422 1715 : ++evaluations;
1423 1715 : minimumEvaluatedGain = std::min( minimumEvaluatedGain, g );
1424 1715 : maximumEvaluatedGain = std::max( maximumEvaluatedGain, g );
1425 1715 : return go->clVariance( *PSDerr, *PSDnoise, g );
1426 : }
1427 : };
1428 :
1429 : // Explicit Instantiation
1430 : extern template class clGainOpt<float>;
1431 :
1432 : extern template class clGainOpt<double>;
1433 :
1434 : extern template class clGainOpt<long double>;
1435 :
1436 : #ifdef HASQUAD
1437 : extern template class clGainOpt<_m_float128>;
1438 : #endif
1439 :
1440 : } // namespace analysis
1441 : } // namespace AO
1442 : } // namespace mx
1443 :
1444 : #endif // clGainOpt_hpp
|
