Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add core State-space classes (#2614)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
2020-08-14 23:40:33 -07:00
committed by GitHub
parent e5b84e2f87
commit 3b283ab9aa
84 changed files with 11747 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp Normal file
View File

@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
namespace drake {
namespace math {
namespace {
TEST(MerweScaledSigmaPointsTest, TestZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points =
sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(0.0, 0.0),
frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0));
EXPECT_TRUE(
(points - frc::MakeMatrix<2, 5>(0.0, 0.00173205, 0.0, -0.00173205, 0.0,
0.0, 0.0, 0.00173205, 0.0, -0.00173205))
.norm() < 1e-3);
}
TEST(MerweScaledSigmaPointsTest, TestNonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points =
sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(1.0, 2.0),
frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 10.0));
EXPECT_TRUE(
(points - frc::MakeMatrix<2, 5>(1.0, 1.00173205, 1.0, 0.998268, 1.0, 2.0,
2.0, 2.00548, 2.0, 1.99452))
.norm() < 1e-3);
}
} // namespace
} // namespace math
} // namespace drake