Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 1 | /*----------------------------------------------------------------------------*/ |
| 2 | /* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ |
| 3 | /* Open Source Software - may be modified and shared by FRC teams. The code */ |
| 4 | /* must be accompanied by the FIRST BSD license file in the root directory of */ |
| 5 | /* the project. */ |
| 6 | /*----------------------------------------------------------------------------*/ |
| 7 | |
| 8 | #include <gtest/gtest.h> |
| 9 | |
| 10 | #include <array> |
| 11 | |
| 12 | #include "Eigen/Core" |
| 13 | #include "frc/StateSpaceUtil.h" |
| 14 | #include "frc/system/RungeKutta.h" |
| 15 | |
| 16 | TEST(StateSpaceUtilTest, MakeMatrix) { |
| 17 | // Column vector |
| 18 | Eigen::Matrix<double, 2, 1> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0); |
| 19 | EXPECT_NEAR(mat1(0), 1.0, 1e-3); |
| 20 | EXPECT_NEAR(mat1(1), 2.0, 1e-3); |
| 21 | |
| 22 | // Row vector |
| 23 | Eigen::Matrix<double, 1, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0); |
| 24 | EXPECT_NEAR(mat2(0), 1.0, 1e-3); |
| 25 | EXPECT_NEAR(mat2(1), 2.0, 1e-3); |
| 26 | |
| 27 | // Square matrix |
| 28 | Eigen::Matrix<double, 2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0); |
| 29 | EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3); |
| 30 | EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3); |
| 31 | EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3); |
| 32 | EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3); |
| 33 | |
| 34 | // Nonsquare matrix with more rows than columns |
| 35 | Eigen::Matrix<double, 3, 2> mat4 = |
| 36 | frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); |
| 37 | EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3); |
| 38 | EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3); |
| 39 | EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3); |
| 40 | EXPECT_NEAR(mat4(1, 1), 4.0, 1e-3); |
| 41 | EXPECT_NEAR(mat4(2, 0), 5.0, 1e-3); |
| 42 | EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3); |
| 43 | |
| 44 | // Nonsquare matrix with more columns than rows |
| 45 | Eigen::Matrix<double, 2, 3> mat5 = |
| 46 | frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); |
| 47 | EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3); |
| 48 | EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3); |
| 49 | EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3); |
| 50 | EXPECT_NEAR(mat5(1, 0), 4.0, 1e-3); |
| 51 | EXPECT_NEAR(mat5(1, 1), 5.0, 1e-3); |
| 52 | EXPECT_NEAR(mat5(1, 2), 6.0, 1e-3); |
| 53 | } |
| 54 | |
| 55 | TEST(StateSpaceUtilTest, CostParameterPack) { |
| 56 | Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0); |
| 57 | EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); |
| 58 | EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); |
| 59 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 60 | EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); |
| 61 | EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3); |
| 62 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 63 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 64 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 65 | EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); |
| 66 | } |
| 67 | |
| 68 | TEST(StateSpaceUtilTest, CostArray) { |
| 69 | Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0}); |
| 70 | EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); |
| 71 | EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); |
| 72 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 73 | EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); |
| 74 | EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3); |
| 75 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 76 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 77 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 78 | EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); |
| 79 | } |
| 80 | |
| 81 | TEST(StateSpaceUtilTest, CovParameterPack) { |
| 82 | Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0); |
| 83 | EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); |
| 84 | EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); |
| 85 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 86 | EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); |
| 87 | EXPECT_NEAR(mat(1, 1), 4.0, 1e-3); |
| 88 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 89 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 90 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 91 | EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); |
| 92 | } |
| 93 | |
| 94 | TEST(StateSpaceUtilTest, CovArray) { |
| 95 | Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0}); |
| 96 | EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); |
| 97 | EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); |
| 98 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 99 | EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); |
| 100 | EXPECT_NEAR(mat(1, 1), 4.0, 1e-3); |
| 101 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 102 | EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); |
| 103 | EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); |
| 104 | EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); |
| 105 | } |
| 106 | |
| 107 | TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) { |
| 108 | Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector(2.0, 3.0); |
| 109 | static_cast<void>(vec); |
| 110 | } |
| 111 | |
| 112 | TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) { |
| 113 | Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0}); |
| 114 | static_cast<void>(vec); |
| 115 | } |
| 116 | |
| 117 | TEST(StateSpaceUtilTest, IsStabilizable) { |
| 118 | Eigen::Matrix<double, 2, 2> A; |
| 119 | Eigen::Matrix<double, 2, 1> B; |
| 120 | B << 0, 1; |
| 121 | |
| 122 | // We separate the result of IsStabilizable from the assertion because |
| 123 | // templates break gtest. |
| 124 | |
| 125 | // First eigenvalue is uncontrollable and unstable. |
| 126 | // Second eigenvalue is controllable and stable. |
| 127 | A << 1.2, 0, 0, 0.5; |
| 128 | bool ret = frc::IsStabilizable<2, 1>(A, B); |
| 129 | EXPECT_FALSE(ret); |
| 130 | |
| 131 | // First eigenvalue is uncontrollable and marginally stable. |
| 132 | // Second eigenvalue is controllable and stable. |
| 133 | A << 1, 0, 0, 0.5; |
| 134 | ret = frc::IsStabilizable<2, 1>(A, B); |
| 135 | EXPECT_FALSE(ret); |
| 136 | |
| 137 | // First eigenvalue is uncontrollable and stable. |
| 138 | // Second eigenvalue is controllable and stable. |
| 139 | A << 0.2, 0, 0, 0.5; |
| 140 | ret = frc::IsStabilizable<2, 1>(A, B); |
| 141 | EXPECT_TRUE(ret); |
| 142 | |
| 143 | // First eigenvalue is uncontrollable and stable. |
| 144 | // Second eigenvalue is controllable and unstable. |
| 145 | A << 0.2, 0, 0, 1.2; |
| 146 | ret = frc::IsStabilizable<2, 1>(A, B); |
| 147 | EXPECT_TRUE(ret); |
| 148 | } |