mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
31 lines
1.1 KiB
C++
31 lines
1.1 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "frc/estimator/MerweScaledSigmaPoints.h"
|
|
|
|
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
|
|
frc::MerweScaledSigmaPoints<2> sigmaPoints;
|
|
auto points = sigmaPoints.SquareRootSigmaPoints(
|
|
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
|
|
|
|
EXPECT_TRUE(
|
|
(points - frc::Matrixd<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, NonzeroMean) {
|
|
frc::MerweScaledSigmaPoints<2> sigmaPoints;
|
|
auto points = sigmaPoints.SquareRootSigmaPoints(
|
|
frc::Vectord<2>{1.0, 2.0},
|
|
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
|
|
|
|
EXPECT_TRUE(
|
|
(points - frc::Matrixd<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);
|
|
}
|