control_systems.cpp

#include "control_systems.h"
#include <vector>
#include <cmath>
#include <iostream>

ControlSystems::ControlSystems(double Ts) : Ts_(Ts) {
    // Assign depth system matrices
    L1_.Amz = {-0.160000000000000, -0.102400000000000, 0.0625000000000000, 0};
    L1_.Bmz = {0.250000000000000, 0};
    L1_.Cmz = {0, 0.409600000000000};
    L1_.Dmz = {0};

    L1_.Aoz = {0.00127644512442100, 0.00584348637373815, 0.0711169366531907, -0.0122243613886095, -0.143786234924726, -0.0248716599459859, -0.00306710916364572, 0.0915082980803851, -0.0674902101996962};
    L1_.Boz = {-0.0280712803471739, 0.0807463935316200, -0.301134220324624, -0.136048660239665, 0.159365126602413, -0.203637567216374};
    L1_.Coz = {-0.500684105200128, -0.0431401328331999, -0.169709739668298};
    L1_.Doz = {0, 0};

    L1_.Lambdaz = {0, 0.409600000000000, 2.24328968990019, 2.17663038437860};
    L1_.Phiinvz = {-47.4417299373405, 8.95117562852876, 48.9021854706762, -0.0139490083626975};

    // Assign yaw system matrices
    L1_.Ampsi = {-0.160000000000000, -0.102400000000000, 0.0625000000000000, 0};
    L1_.Bmpsi = {0.250000000000000, 0};
    L1_.Cmpsi = {0, 0.409600000000000};
    L1_.Dmpsi = {0};

    L1_.Aopsi = {-0.0177132711750731, -0.0116117565420850, -0.0317457476431568, 0.00775078058815142, -0.00326665848763519, -0.00732916771046503, -0.000974929943737770, -0.000839651422519338, -0.00902007033729174};
    L1_.Bopsi = {0.0738968318064207, 0.140531274977130, 0.0199569970420227, 0.0536715440167758, 0.0472377594657531, 0.112690439144517};
    L1_.Copsi = {-0.0488807363804482, -0.288320483926326, 0.198276605530847};
    L1_.Dopsi = {0, 0};

    L1_.Lambdapsi = {0, 0.409600000000000, 2.24328968990019, 2.17663038437860};
    L1_.Phiinvpsi = {-47.4417299373405, 8.95117562852876, 48.9021854706762, -0.0139490083626975};
}

void ControlSystems::computeDepthSystem() {
    // In this implementation, the matrices are already assigned in the constructor
}

void ControlSystems::computeYawSystem() {
    // In this implementation, the matrices are already assigned in the constructor
}

const L1Struct& ControlSystems::getL1() const {
    return L1_;
}