<commit_before>//--------------------------------------------------------------------------
//
// Copyright (C) 2014 <NAME>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
//
//--------------------------------------------------------------------------

/** \file
 * Sample controlling a 3rd order system across a unit step in the setpoint.
 *
 * This sample can be used to test controller behavior against known good
 * results.  For example, those presented in Figure 10.2 within <a
 * href="http://www.cds.caltech.edu/~murray/amwiki/index.php/PID_Control">
 * Chapter 10</a> of <a href="http://www.worldcat.org/isbn/0691135762">Astrom
 * and Murray</a>.
 */

#include <getopt.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "helm.h"

/**
 * Advance the temporal state of a model given by transfer function \f$
 * \frac{y(s)}{u(s)} = \frac{b_0}{s^3 + a_2 s^2 + a_1 s + a_0} \f$.
 *
 * Given the process transfer function
 * \f{align}{
 *     \frac{y(s)}{u(s)} = \frac{b_0}{s^3 + a_2 s^2 + a_1 s + a_0}
 * \f}
 * a matching state space model consisting of 1st order differential equations
 * <a
 * href="http://lpsa.swarthmore.edu/Representations/SysRepTransformations/SysRepTransfAll.html">
 * can be derived</a> with the form
 * \f{align}{
 *     \frac{\mathrm{d}}{\mathrm{d}t}
 *     \begin{bmatrix} y_0(t) \\ y_1(t) \\ y_2(t) \end{bmatrix}
 *     &=
 *     \begin{bmatrix}
 *            0 &    1 &    0 \\
 *            0 &    0 &    1 \\
 *         -a_0 & -a_1 & -a_2
 *     \end{bmatrix}
 *     \begin{bmatrix} y_0(t) \\ y_1(t) \\ y_2(t) \end{bmatrix}
 *     +
 *     \begin{bmatrix}   0 \\   0 \\ b_0 \end{bmatrix} u(t)
 * \f}
 * for constants \f$a_0\f$, \f$a_1\f$, \f$a_2\f$, and \f$b_0\f$ and time-varying
 * input data \f$u(t)\f$.  Using a semi-implicit Euler integration scheme,
 * \f{align}{
 *     \vec{y}\left(t_{i+1}\right)
 *     &=
 *       \vec{y}\left(t_i\right)
 *     + h \vec{f}\left(\vec{y}\left(t_{i+1}\right), u\left(t_i\right)\right)
 *     ,
 * \f}
 * yields a constant-coefficient linear problem for advancing by time \f$h\f$:
 * \f{align}{
 *     \begin{bmatrix}
 *            1 &   -h &      0 \\
 *            0 &    1 &     -h \\
 *         ha_0 & ha_1 & 1+ha_2
 *     \end{bmatrix}
 *     \begin{bmatrix} y_0(t_{i+1})\\y_1(t_{i+1})\\y_2(t_{i+1}) \end{bmatrix}
 *     &=
 *     \begin{bmatrix} y_0(t_i) \\ y_1(t_i) \\ y_2(t_i) \end{bmatrix}
 *     +
 *     \begin{bmatrix}   0 \\   0 \\ b_0 \end{bmatrix} u(t_i)
 * \f}
 * Left multiplying by the matrix cofactor and dividing by the determinant gives
 * a form amenable to computation,
 * \f{align}{
 *     \begin{bmatrix} y_0(t_{i+1})\\y_1(t_{i+1})\\y_2(t_{i+1}) \end{bmatrix}
 *     &=
 *     \frac{
 *       \begin{bmatrix}
 *        h (a_2+a_1 h)+1 & h (a_2 h+1)    & h^2 \\
 *        -a_0 h^2        & a_2 h+1        & h \\
 *        -a_0 h          & -h (a_1+a_0 h) & 1
 *       \end{bmatrix}
 *       \left(
 *           \begin{bmatrix} y_0(t_i) \\ y_1(t_i) \\ y_2(t_i) \end{bmatrix}
 *           +
 *           \begin{bmatrix}   0 \\   0 \\ b_0 \end{bmatrix} u(t_i)
 *       \right)
 *     }{h (h (a_0 h+a_1)+a_2)+1}
 *     .
 * \f}
 * This routine advances \f$\vec{y}(t)\f$ to \f$\vec{y}(t + h)\f$ using
 * the above result.
 *
 * @param[in    ] h Time step \f$h\f$ to be taken.
 * @param[in    ] a Coefficients \f$a_0\f$, \f$a_1\f$, and \f$a_2\f$.
 * @param[in    ] b Coefficient \f$b_0\f$.
 * @param[in    ] u Input \f$u(t)\f$.
 * @param[in,out] y On input,  state \f$y_0(t)\f$, \f$y_1(t)\f$,
 *                  and \f$y_2(t)\f$.  On output, state \f$y_0(t+h)\f$,
 *                  \f$y_1(t+h)\f$, and \f$y_2(t+h)\f$.
 */
static
void
advance(const double h,
        const double a[restrict static 3],
        const double b[restrict static 1],
        const double u[restrict static 1],
              double y[restrict static 3])
{
    const double rhs[3] = {
        y[0],
        y[1],
        y[2] + b[0]*u[0]
    };
    const double cof[3][3] = {
        { h*(a[2] + a[1]*h) + 1 , h*(a[2]*h + 1)     , h*h },
        { -a[0]*h*h             , a[2]*h + 1         , h   },
        { -a[0]*h               , -h*(a[1] + a[0]*h) , 1   }
    };
    const double det = h*(h*(a[0]*h + a[1]) + a[2]) + 1;

    for (int i = 0; i < 3; ++i) {
        y[i] = 0;
        for (int j = 0; j < 3; ++j) {
            y[i] += cof[i][j]*rhs[j];
        }
        y[i] /= det;
    }
}

static const double default_a[3] = {1, 3, 3}; ///< Default process parameters
static const double default_b[1] = {1};       ///< Default process parameters
static const double default_f    = 0.01;      ///< Default filter time scale
static const double default_kd   = 0;         ///< Default derivative gain
static const double default_ki   = 0.2;       ///< Default integration gain
static const double default_kp   = 1;         ///< Default proportional gain
static const double default_r    = 1;         ///< Default reference value
static const double default_t    = 0.05;      ///< Default time step size
static const double default_T    = 20;        ///< Default final time

/** Print usage on the given stream. */
static
void
print_usage(const char *arg0, FILE *out)
{
    fprintf(out, "Usage: %s [OPTION...]\n", arg0);
    fprintf(out, "Control 3rd-order system across a setpoint step change.\n");
    fprintf(out, "Output is tab-delimited t, u, y[0], y[1], y[2].\n");
    fputc('\n', out);
    fprintf(out, "Process transfer function"<commit_msg>Pick a stable default time step size<commit_after>//--------------------------------------------------------------------------
//
// Copyright (C) 2014 <NAME>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
//
//--------------------------------------------------------------------------

/** \file
 * Sample controlling a 3rd order system across a unit step in the setpoint.
 *
 * This sample can be used to test controller behavior against known good
 * results.  For example, those presented in Figure 10.2 within <a
 * href="http://www.cds.caltech.edu/~murray/amwiki/index.php/PID_Control">
 * Chapter 10</a> of <a href="http://www.worldcat.org/isbn/0691135762">Astrom
 * and Murray</a>.
 */

#include <getopt.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "helm.h"

/**
 * Advance the temporal state of a model given by transfer function \f$
 * \frac{y(s)}{u(s)} = \frac{b_0}{s^3 + a_2 s^2 + a_1 s + a_0} \f$.
 *
 * Given the process transfer function
 * \f{align}{
 *     \frac{y(s)}{u(s)} = \frac{b_0}{s^3 + a_2 s^2 + a_1 s + a_0}
 * \f}
 * a matching state space model consisting of 1st order differential equations
 * <a
 * href="http://lpsa.swarthmore.edu/Representations/SysRepTransformations/SysRepTransfAll.html">
 * can be derived</a> with the form
 * \f{align}{
 *     \frac{\mathrm{d}}{\mathrm{d}t}
 *     \begin{bmatrix} y_0(t) \\ y_1(t) \\ y_2(t) \end{bmatrix}
 *     &=
 *     \begin{bmatrix}
 *            0 &    1 &    0 \\
 *            0 &    0 &    1 \\
 *         -a_0 & -a_1 & -a_2
 *     \end{bmatrix}
 *     \begin{bmatrix} y_0(t) \\ y_1(t) \\ y_2(t) \end{bmatrix}
 *     +
 *     \begin{bmatrix}   0 \\   0 \\ b_0 \end{bmatrix} u(t)
 * \f}
 * for constants \f$a_0\f$, \f$a_1\f$, \f$a_2\f$, and \f$b_0\f$ and time-varying
 * input data \f$u(t)\f$.  Using a semi-implicit Euler integration scheme,
 * \f{align}{
 *     \vec{y}\left(t_{i+1}\right)
 *     &=
 *       \vec{y}\left(t_i\right)
 *     + h \vec{f}\left(\vec{y}\left(t_{i+1}\right), u\left(t_i\right)\right)
 *     ,
 * \f}
 * yields a constant-coefficient linear problem for advancing by time \f$h\f$:
 * \f{align}{
 *     \begin{bmatrix}
 *            1 &   -h &      0 \\
 *            0 &    1 &     -h \\
 *         ha_0 & ha_1 & 1+ha_2
 *     \end{bmatrix}
 *     \begin{bmatrix} y_0(t_{i+1})\\y_1(t_{i+1})\\y_2(t_{i+1}) \end{bmatrix}
 *     &=
 *     \begin{bmatrix} y_0(t_i) \\ y_1(t_i) \\ y_2(t_i) \end{bmatrix}
 *     +
 *     \begin{bmatrix}   0 \\   0 \\ b_0 \end{bmatrix} u(t_i)
 * \f}
 * Left multiplying by the matrix cofactor and dividing by the determinant gives
 * a form amenable to computation,
 * \f{align}{
 *     \begin{bmatrix} y_0(t_{i+1})\\y_1(t_{i+1})\\y_2(t_{i+1}) \end{bmatrix}
 *     &=
 *     \frac{
 *       \begin{bmatrix}
 *        h (a_2+a_1 h)+1 & h (a_2 h+1)    & h^2 \\
 *        -a_0 h^2        & a_2 h+1        & h \\
 *        -a_0 h          & -h (a_1+a_0 h) & 1
 *       \end{bmatrix}
 *       \left(
 *           \begin{bmatrix} y_0(t_i) \\ y_1(t_i) \\ y_2(t_i) \end{bmatrix}
 *           +
 *           \begin{bmatrix}   0 \\   0 \\ b_0 \end{bmatrix} u(t_i)
 *       \right)
 *     }{h (h (a_0 h+a_1)+a_2)+1}
 *     .
 * \f}
 * This routine advances \f$\vec{y}(t)\f$ to \f$\vec{y}(t + h)\f$ using
 * the above result.
 *
 * @param[in    ] h Time step \f$h\f$ to be taken.
 * @param[in    ] a Coefficients \f$a_0\f$, \f$a_1\f$, and \f$a_2\f$.
 * @param[in    ] b Coefficient \f$b_0\f$.
 * @param[in    ] u Input \f$u(t)\f$.
 * @param[in,out] y On input,  state \f$y_0(t)\f$, \f$y_1(t)\f$,
 *                  and \f$y_2(t)\f$.  On output, state \f$y_0(t+h)\f$,
 *                  \f$y_1(t+h)\f$, and \f$y_2(t+h)\f$.
 */
static
void
advance(const double h,
        const double a[restrict static 3],
        const double b[restrict static 1],
        const double u[restrict static 1],
              double y[restrict static 3])
{
    const double rhs[3] = {
        y[0],
        y[1],
        y[2] + b[0]*u[0]
    };
    const double cof[3][3] = {
        { h*(a[2] + a[1]*h) + 1 , h*(a[2]*h + 1)     , h*h },
        { -a[0]*h*h             , a[2]*h + 1         , h   },
        { -a[0]*h               , -h*(a[1] + a[0]*h) , 1   }
    };
    const double det = h*(h*(a[0]*h + a[1]) + a[2]) + 1;

    for (int i = 0; i < 3; ++i) {
        y[i] = 0;
        for (int j = 0; j < 3; ++j) {
            y[i] += cof[i][j]*rhs[j];
        }
        y[i] /= det;
    }
}

static const double default_a[3] = {1, 3, 3}; ///< Default process parameters
static const double default_b[1] = {1};       ///< Default process parameters
static const double default_f    = 0.01;      ///< Default filter time scale
static const double default_kd   = 0;         ///< Default derivative gain
static const double default_ki   = 0.2;       ///< Default integration gain
static const double default_kp   = 1;         ///< Default proportional gain
static const double default_r    = 1;         ///< Default reference value
static const double default_t    = 0.25;      ///< Default time step size
static const double default_T    = 20;        ///< Default final time

/** Print usage on the given stream. */
static
void
print_usage(const char *arg0, FILE *out)
{
    fprintf(out, "Usage: %s [OPTION...]\n", arg0);
    fprintf(out, "Control 3rd-order system across a setpoint step change.\n");
    fprintf(out, "Output is tab-delimited t, u, y[0], y[1], y[2].\n");
    fputc('\n', out);
    fprintf(out, "Process transfer function"
{'fraction_non_alphanumeric': 0.20921322284594138, 'fraction_numerical': 0.03189877929252428, 'mean_word_length': 1.9010080124063065, 'pattern_counts': {'":': 0, '<': 47, '<?xml version=': 0, '>': 25, 'https://': 0, 'lorem ipsum': 0, 'www.': 4, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '688231', 'n_tokens_mistral': 4482, 'n_tokens_neox': 4298, 'n_words': 1087}