Skip to content

tomlankhorst/control

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

90 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

C++ Control

API Docs Github

Control functionality in C++

Classic PIDF control

P, PI, PD and PID control:

#include <control/classic/pid.h>

using P   = control::classic::P<int>;
using PI  = control::classic::PI<float>;
using PD  = control::classic::PI<double>;
using PID = control::classic::PID<double>;

// ...

// Ts = 1.0s, K = 1.0, Ti = 2.0
PI controller(1, 1, 2);

for( int i = 0; i < 10; i++ )
  std::cout << controller.step(1.0) << std::endl;

// 1.5 2.0 2.5 ...

Based on trapezoidal (Tustin) discretizations of the standard (serial) PID controller.

PI, PD and PID are use Biquads and expose functionality like .poles().

Biquad Digital Filters

Biquads, or Second Order Sections (SOS), are transfer functions consisting of a ratio of two quadratic polynomials. With z operator:

       b0 + b1 z^-1 + b2 z^-2
H(z) = ----------------------
        1 + a1 z^-1 + a2 z^-2

(normalized by a0)

Biquads can be chained to obtain higher-order transfer-functions.

The implementation of Biquads in this library is based on the Direct-form II transposed implementation.

// 2-nd order Butterworth LP filter with Wc =~ 0.1 (* half sample-rate)
float b0 = 0.02, b1 = 0.04, b2 = 0.02;
float a1 = -1.56, a2 = 0.64;
Biquad<float> b(b0, b1, b2, a1, a2);

for(int i=0; i<5; i++)
    std::cout << b.step(1) << std::endl;
// 0.02.., 0.09.., 0.22.., ...

Retrieving the poles of a Biquad:

// Tuple of two std::complex<T>
auto ps = b.poles();
// std::complex<T>
auto p1 = std::get<0>(ps);
auto p2 = std::get<1>(ps);

g-h-k filters (alpha-beta-gamma filters)

Implements g-h-k filter.

using namespace control::ghk;
                         
// initial conditions x0, dx0, ddx0
auto x = state<double>{ 1, 0, 0 };
auto Ts = 0.01; 
// sigma_w and sigma_v noise variance
auto c = parameterize::optimal_gaussian(0.1, 1, Ts);
                                            
auto z = 0.5; // measurement
auto& [corr, pred] = correct_predict(c, x, z, Ts);
x = corr;                                         

System Identification

Pseudo-Random Binary Signal

Expose a system to a PRBS to identify its transfer characteristics.

control::ident::PRBS<int> P;
auto i = P.get(); // 1 of -1

State-Space Systems

State-spaces allow representation of LTI discretized systems with Nx states, Nu inputs and Ny outputs.

// 2nd order SISO state-space
using ss2 = control::system::ss<float,2>;

ss2::TA A << 1, 1, 0, 1;
ss2::TA B << 0, 1;
ss2::TC C << 1, 0;
ss2::TD D << 0;
ss2 P(A,B,C,D);

// 3th order MIMO state-space
using ss3 = control::system::ss<float,3,2,2>;

ss3::TA A << /* ... */ ;
// ...

ss3 P3(A,B,C,D);

ss3::Tu u << 1, 2;
auto y = P3.step(u);
// y(0), y(1)

This functionality is based upon the Eigen3 Matrix math library. Eigen takes care of target-specific vectorization!

Tests

mkdir build && cd build
cmake .. -DTESTS=ON
make
./controltests

There's a short article about this library.