Program Listing for File StateSpace.h#
↰ Return to documentation for file (include/Karana/Math/StateSpace.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains the declarations for the Jacobian class, StateSpace class, and related Eigen
* functors.
*/
#pragma once
#include "Karana/Math/Defs.h"
#include <functional>
#include <memory>
#include <unsupported/Eigen/NumericalDiff>
#include <variant>
namespace Karana::Math {
/**
* @brief Generic functor base class required by Eigen's NumericalDiff package.
* @tparam Scalar_ Scalar type.
* @tparam NX Number of inputs at compile time.
* @tparam NY Number of values at compile time.
*/
template <typename Scalar_ = double, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct NumDiffBaseFunctor {
/// Scalar type used by the functor.
using Scalar = Scalar_;
enum {
InputsAtCompileTime = NX, // NOLINT(readability-identifier-naming)
ValuesAtCompileTime = NY // NOLINT(readability-identifier-naming)
};
/// Input type used by the functor. It has type Scalar and is size NX x 1
using InputType = Eigen::Matrix<Scalar, InputsAtCompileTime, 1>;
/// Value (output) type used by the functor. It has type Scalar and is size NY x 1
using ValueType = Eigen::Matrix<Scalar, ValuesAtCompileTime, 1>;
/// Jacobian type used by the functor. It has type Scalar and is size NY x Nx
using JacobianType =
Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime, Eigen::RowMajor>;
/// Internal variable that holds the number of inputs
const int _inputs; // NOLINT(readability-identifier-naming)
/// Internal variable that holds the number of values (outputs)
const int _values; // NOLINT(readability-identifier-naming)
/**
* @brief Constructor for the Functor.
* @param inputs Number of inputs.
* @param values Number of outputs.
*/
NumDiffBaseFunctor(int inputs, int values)
: _inputs(inputs)
, _values(values) {}
/**
* @brief Get the number of inputs.
* @return Number of inputs.
*/
int inputs() const { return _inputs; }
/**
* @brief Get the number of outputs.
* @return Number of outputs.
*/
int values() const { return _values; }
};
/// Alias for the function type used by NumDiffFunctor
using fn = std::function<void(const Vec &, Vec &)>;
/**
* @brief Functor for numerical differencing with Eigen's NumDiff package.
*/
struct NumDiffFunctor : NumDiffBaseFunctor<double, Eigen::Dynamic, Eigen::Dynamic> {
/// Alias to the base functor
using Base = NumDiffBaseFunctor<double, Eigen::Dynamic, Eigen::Dynamic>;
/// Function to differentiate numerically
fn _f; // NOLINT(readability-identifier-naming)
/**
* @brief Constructor for NumDiffFunctor.
* @param inputs Number of inputs.
* @param values Number of outputs.
* @param f Function to differentiate.
*/
NumDiffFunctor(int inputs, int values, fn f)
: NumDiffBaseFunctor(inputs, values)
, _f(f){};
/**
* @brief Evaluate the function.
* @param x Input vector.
* @param fvec Output vector.
* @return Status code (0 for success).
*/
int operator()(const Vec &x, Vec &fvec) const {
_f(x, fvec);
return 0;
}
};
/// Functors for each of the numerical differencing options in Eigen
using num_diff_functor_type =
std::variant<Eigen::NumericalDiff<NumDiffFunctor, Eigen::Forward>,
Eigen::NumericalDiff<NumDiffFunctor, Eigen::Central>>;
/**
* @class Jacobian
* @brief Class used to create a Jacobian for a given function using numerical differencing.
*/
class Jacobian {
public:
/**
* @brief Creates a Jacobian class. This class outputs the Jacobian of the input function f.
*
* @param input_dim The number of inputs to f.
* @param value_dim The number of outputs of f.
* @param f The function to use finite difference to find the Jacobian of.
* @param mode The finite difference mode. "forward" for forward difference and "central"
* for central difference.
*/
Jacobian(int input_dim, int value_dim, fn f, std::string_view mode = "forward");
/**
* @brief Get the function that this Jacobian class calculates the jacobian of.
* @returns f The function that this Jacobian class calculates the jacobian of.
*/
fn getF();
/**
* @brief Set the function that this class calculates the jacobian of.
* @param f The function that you want to calculate the jacobian of.
*/
void setF(fn f);
/**
* @brief Get the finite difference mode.
* @returns f The finite difference mode.
*/
std::string_view getMode();
/**
* @brief Sets finite difference mode.
* @param mode The finite difference mode. One of "forward" or "central".
*/
void setMode(std::string_view mode);
/**
* @brief Calculate the jacobian of the supplied function.
*
* @param x The input vector we want to calculate the jacobian with respect to.
* @return The jacobian of the supplied function about x.
*/
Mat operator()(const Vec &x) const;
private:
/// The number of inputs to the function.
int _input_dim;
/// The number of outputs of the function.
int _value_dim;
/// The function to take the jacobian of.
fn _f;
std::string _mode;
/// A unique pointer to the functor used to calculate the Jacobian.
std::unique_ptr<num_diff_functor_type> _func;
/**
* @brief Sets the functor for numerical differences.
*/
void _setFunctor();
/**
* @brief Jacobian computation for forward difference.
*/
void _jacobian(Eigen::NumericalDiff<NumDiffFunctor, Eigen::Forward> &func,
const Vec &x,
Mat &J) const;
/**
* @brief Jacobian computation for central difference.
*/
void _jacobian(Eigen::NumericalDiff<NumDiffFunctor, Eigen::Central> &func,
const Vec &x,
Mat &J) const;
};
/// Function type used by the StateSpace model.
using lin_fn = std::function<void(const Vec &, const Vec &, Eigen::Ref<Vec>)>;
/**
* @class StateSpace
* @brief Use provided non-linear equations to create a state space model of the form:
* x_dot = A*x + B*u, y = C*x + D*u from a nonlinear set of
* equations of the form x_dot = f(x,u), y = g(x,u)
*/
class StateSpace {
public:
/**
* @brief Create a StateSpace class. This uses non-linear equations to create a
* state space model of the form: x_dot = A*x + B*u, y = C*x + D*u from a nonlinear set of
* equations of the form x_dot = f(x,u), y = g(x,u)
* @param num_inputs The number of inputs, u, in the state space model.
* @param num_outputs The number of outputs, y, in the state space model.
* @param num_states The number of states, x, in the state space model.
* @param state_deriv_fn The non-linear function, f, used to create the A and B matrices.
* @param output_fn The non-linear function, g, used to create the C and D matrices.
* @param mode The numerical differencing technique to use. "forward" for forward
* differencing and "central" for central differencing.
*/
StateSpace(int num_inputs,
int num_outputs,
int num_states,
lin_fn state_deriv_fn,
lin_fn output_fn,
std::string_view mode = "forward");
/**
* @brief State space return type. This holds the A, B, C, and D matrices that make up the
* state space model.
*/
struct SS {
/// A matrix of the state space model
Mat A;
/// B matrix of the state space model
Mat B;
/// C matrix of the state space model
Mat C;
/// D matrix of the state space model
Mat D;
};
/**
* @brief Generate a state space model by linearizing the system about x and u.
* @param x The state vector, x, to generate about.
* @param u The input vector, u, to generate about.
* @return A structure with the matrices for the state space model.
*/
SS generate(const Vec &x, const Vec &u);
private:
int _num_inputs;
int _num_outputs;
int _num_states;
lin_fn _state_deriv_fn;
lin_fn _output_fn;
std::string _mode;
};
} // namespace Karana::Math