blob: 24e9cf296b47dae2a2b30c824e4bb199c34c9681 [file] [log] [blame]
Austin Schuh1e69f942020-11-14 15:06:14 -08001/*----------------------------------------------------------------------------*/
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
16TEST(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
55TEST(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
68TEST(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
81TEST(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
94TEST(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
107TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
108 Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
109 static_cast<void>(vec);
110}
111
112TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
113 Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
114 static_cast<void>(vec);
115}
116
117TEST(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}