forked from Archive/PX4-Autopilot
yaw_est: use error-state covariance prediction
Convergence improvements in high yaw rate conditions
This commit is contained in:
bresch
Daniel Agar
parent
32127ca789
commit
00568985c0
|
|
@ -283,6 +283,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
|
|||
const float sin_yaw = sinf(_ekf_gsf[model_index].X(2));
|
||||
const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw;
|
||||
const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw;
|
||||
const float daz = Vector3f(_ahrs_ekf_gsf[model_index].R * delta_ang)(2);
|
||||
|
||||
// delta velocity process noise double if we're not in air
|
||||
const float accel_noise = in_air ? _accel_noise : 2.f * _accel_noise;
|
||||
|
|
@ -292,7 +293,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
|
|||
const float d_ang_var = sq(_gyro_noise * delta_ang_dt);
|
||||
|
||||
sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P,
|
||||
Vector2f(dvx, dvy), d_vel_var, d_ang_var, &_ekf_gsf[model_index].P);
|
||||
Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var, &_ekf_gsf[model_index].P);
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Copyright (c) 2022 PX4 Development Team
|
||||
Copyright (c) 2022-2023 PX4 Development Team
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
|
@ -37,51 +37,86 @@ import symforce
|
|||
symforce.set_epsilon_to_symbol()
|
||||
|
||||
import symforce.symbolic as sf
|
||||
from symforce.values import Values
|
||||
from derivation_utils import *
|
||||
|
||||
class State:
|
||||
vx = 0
|
||||
vy = 1
|
||||
yaw = 2
|
||||
n_states = 3
|
||||
State = Values(
|
||||
vel = sf.V2(),
|
||||
R = sf.Rot2() # 2D rotation to handle angle wrap
|
||||
)
|
||||
|
||||
class VState(sf.Matrix):
|
||||
SHAPE = (State.n_states, 1)
|
||||
class VTangent(sf.Matrix):
|
||||
SHAPE = (State.tangent_dim(), 1)
|
||||
|
||||
class MState(sf.Matrix):
|
||||
SHAPE = (State.n_states, State.n_states)
|
||||
class MTangent(sf.Matrix):
|
||||
SHAPE = (State.tangent_dim(), State.tangent_dim())
|
||||
|
||||
def rot2_small_angle(angle: sf.V1):
|
||||
# Approximation for small "delta angles" to avoid trigonometric functions
|
||||
return sf.Rot2(sf.Complex(1, angle[0]))
|
||||
|
||||
def yaw_est_predict_covariance(
|
||||
state: VState,
|
||||
P: MState,
|
||||
state: VTangent,
|
||||
P: MTangent,
|
||||
d_vel: sf.V2,
|
||||
d_vel_var: sf.Scalar,
|
||||
d_ang_var: sf.Scalar
|
||||
d_ang: sf.Scalar,
|
||||
d_ang_var: sf.Scalar,
|
||||
):
|
||||
d_ang = sf.Symbol("d_ang") # does not appear in the jacobians
|
||||
state = State.from_tangent(state)
|
||||
d_ang = sf.V1(d_ang) # cast to vector to gain group properties (e.g.: to_tangent)
|
||||
|
||||
# derive the body to nav direction transformation matrix
|
||||
Tbn = sf.Matrix([[sf.cos(state[State.yaw]) , -sf.sin(state[State.yaw])],
|
||||
[sf.sin(state[State.yaw]) , sf.cos(state[State.yaw])]])
|
||||
state_error = Values(
|
||||
vel = sf.V2.symbolic("delta_vel"),
|
||||
yaw = sf.V1.symbolic("delta_yaw")
|
||||
)
|
||||
|
||||
# attitude update equation
|
||||
yaw_new = state[State.yaw] + d_ang
|
||||
# True state kinematics
|
||||
state_t = Values(
|
||||
vel = state["vel"] + state_error["vel"],
|
||||
R = state["R"] * rot2_small_angle(state_error["yaw"])
|
||||
)
|
||||
|
||||
# velocity update equations
|
||||
v_new = sf.V2(state[State.vx], state[State.vy]) + Tbn * d_vel
|
||||
noise = Values(
|
||||
d_vel = sf.V2.symbolic("a_n"),
|
||||
d_ang = sf.V1.symbolic("w_n"),
|
||||
)
|
||||
|
||||
# Define vector of process equations
|
||||
state_new = VState.block_matrix([[v_new], [sf.V1(yaw_new)]])
|
||||
input_t = Values(
|
||||
d_vel = d_vel - noise["d_vel"],
|
||||
d_ang = d_ang - noise["d_ang"]
|
||||
)
|
||||
|
||||
state_t_pred = Values(
|
||||
vel = state_t["vel"] + state_t["R"] * input_t["d_vel"],
|
||||
R = state_t["R"] * rot2_small_angle(input_t["d_ang"])
|
||||
)
|
||||
|
||||
# Nominal state kinematics
|
||||
state_pred = Values(
|
||||
vel = state["vel"] + state["R"] * d_vel,
|
||||
R = state["R"] * rot2_small_angle(d_ang)
|
||||
)
|
||||
|
||||
# Error state kinematics
|
||||
delta_rot = (state_pred["R"].inverse() * state_t_pred["R"])
|
||||
state_error_pred = Values(
|
||||
vel = state_t_pred["vel"] - state_pred["vel"],
|
||||
yaw = sf.simplify(delta_rot.z.imag) # small angle appriximation; use simplify to cancel R.T*R
|
||||
)
|
||||
|
||||
zero_state_error = {state_error[key]: state_error[key].zero() for key in state_error.keys()}
|
||||
zero_noise = {noise[key]: noise[key].zero() for key in noise.keys()}
|
||||
|
||||
# Calculate state transition matrix
|
||||
F = state_new.jacobian(state)
|
||||
F = VTangent(state_error_pred.to_storage()).jacobian(state_error).subs(zero_state_error).subs(zero_noise)
|
||||
|
||||
# Derive the covariance prediction equations
|
||||
# Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
|
||||
# velocities, after bias effects have been removed.
|
||||
|
||||
# derive the control(disturbance) influence matrix from IMU noise to state noise
|
||||
G = state_new.jacobian(sf.V3.block_matrix([[d_vel], [sf.V1(d_ang)]]))
|
||||
# derive the control(disturbance) influence matrix from IMU noise to error-state noise
|
||||
G = VTangent(state_error_pred.to_storage()).jacobian(noise).subs(zero_state_error).subs(zero_noise)
|
||||
|
||||
# derive the state error matrix
|
||||
var_u = sf.Matrix.diag([d_vel_var, d_vel_var, d_ang_var])
|
||||
|
|
@ -93,15 +128,15 @@ def yaw_est_predict_covariance(
|
|||
# Generate the equations for the upper triangular matrix and the diagonal only
|
||||
# Since the matrix is symmetric, the lower triangle does not need to be derived
|
||||
# and can simply be copied in the implementation
|
||||
for index in range(State.n_states):
|
||||
for j in range(State.n_states):
|
||||
for index in range(State.tangent_dim()):
|
||||
for j in range(State.tangent_dim()):
|
||||
if index > j:
|
||||
P_new[index,j] = 0
|
||||
|
||||
return P_new
|
||||
|
||||
def yaw_est_compute_measurement_update(
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
vel_obs_var: sf.Scalar,
|
||||
epsilon : sf.Scalar
|
||||
):
|
||||
|
|
@ -123,8 +158,8 @@ def yaw_est_compute_measurement_update(
|
|||
# Generate the equations for the upper triangular matrix and the diagonal only
|
||||
# Since the matrix is symmetric, the lower triangle does not need to be derived
|
||||
# and can simply be copied in the implementation
|
||||
for index in range(State.n_states):
|
||||
for j in range(State.n_states):
|
||||
for index in range(State.tangent_dim()):
|
||||
for j in range(State.tangent_dim()):
|
||||
if index > j:
|
||||
P_new[index,j] = 0
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ namespace sym {
|
|||
* P: Matrix33
|
||||
* d_vel: Matrix21
|
||||
* d_vel_var: Scalar
|
||||
* d_ang: Scalar
|
||||
* d_ang_var: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
|
|
@ -29,13 +30,13 @@ template <typename Scalar>
|
|||
void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
|
||||
const matrix::Matrix<Scalar, 3, 3>& P,
|
||||
const matrix::Matrix<Scalar, 2, 1>& d_vel, const Scalar d_vel_var,
|
||||
const Scalar d_ang_var,
|
||||
const Scalar d_ang, const Scalar d_ang_var,
|
||||
matrix::Matrix<Scalar, 3, 3>* const P_new = nullptr) {
|
||||
// Total ops: 33
|
||||
// Total ops: 39
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (7)
|
||||
// Intermediate terms (8)
|
||||
const Scalar _tmp0 = std::cos(state(2, 0));
|
||||
const Scalar _tmp1 = std::sin(state(2, 0));
|
||||
const Scalar _tmp2 = -_tmp0 * d_vel(1, 0) - _tmp1 * d_vel(0, 0);
|
||||
|
|
@ -44,6 +45,7 @@ void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
|
|||
std::pow(_tmp0, Scalar(2)) * d_vel_var + std::pow(_tmp1, Scalar(2)) * d_vel_var;
|
||||
const Scalar _tmp5 = _tmp0 * d_vel(0, 0) - _tmp1 * d_vel(1, 0);
|
||||
const Scalar _tmp6 = P(1, 2) + P(2, 2) * _tmp5;
|
||||
const Scalar _tmp7 = std::pow(d_ang, Scalar(2)) + 1;
|
||||
|
||||
// Output terms (1)
|
||||
if (P_new != nullptr) {
|
||||
|
|
@ -55,9 +57,9 @@ void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
|
|||
_p_new(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5;
|
||||
_p_new(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6;
|
||||
_p_new(2, 1) = 0;
|
||||
_p_new(0, 2) = _tmp3;
|
||||
_p_new(1, 2) = _tmp6;
|
||||
_p_new(2, 2) = P(2, 2) + d_ang_var;
|
||||
_p_new(0, 2) = _tmp3 * _tmp7;
|
||||
_p_new(1, 2) = _tmp6 * _tmp7;
|
||||
_p_new(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -57,7 +57,6 @@ px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_senso
|
|||
px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_yaw_estimator_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_yaw_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_SensorRangeFinder.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_drag_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
|
|
|
|||
|
|
@ -1,230 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include "EKF/ekf.h"
|
||||
#include "test_helper/comparison_helper.h"
|
||||
|
||||
#include "../EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h"
|
||||
|
||||
using namespace matrix;
|
||||
typedef SquareMatrix<float, 3> SquareMatrix3f;
|
||||
|
||||
SquareMatrix3f createRandomCovarianceMatrix3f()
|
||||
{
|
||||
// Create a symmetric square matrix
|
||||
SquareMatrix3f P;
|
||||
|
||||
for (int col = 0; col <= 2; col++) {
|
||||
for (int row = 0; row <= col; row++) {
|
||||
if (row == col) {
|
||||
P(row, col) = randf();
|
||||
|
||||
} else {
|
||||
P(col, row) = P(row, col) = 2.0f * (randf() - 0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Make it positive definite
|
||||
P = P.transpose() * P;
|
||||
|
||||
return P;
|
||||
}
|
||||
|
||||
void sympyYawEstUpdate(const SquareMatrix3f &P, float velObsVar, SquareMatrix<float, 2> &S_inverse,
|
||||
float &S_det_inverse, Matrix<float, 3, 2> &K, SquareMatrix3f &P_new)
|
||||
{
|
||||
const float P00 = P(0, 0);
|
||||
const float P01 = P(0, 1);
|
||||
const float P02 = P(0, 2);
|
||||
const float P11 = P(1, 1);
|
||||
const float P12 = P(1, 2);
|
||||
const float P22 = P(2, 2);
|
||||
|
||||
// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
|
||||
const float t0 = powf(P01, 2.f);
|
||||
const float t1 = -t0;
|
||||
const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + powf(velObsVar, 2.f);
|
||||
|
||||
if (fabsf(t2) < 1e-6f) {
|
||||
return;
|
||||
}
|
||||
|
||||
const float t3 = 1.0F / t2;
|
||||
const float t4 = P11 + velObsVar;
|
||||
const float t5 = P01 * t3;
|
||||
const float t6 = -t5;
|
||||
const float t7 = P00 + velObsVar;
|
||||
const float t8 = P00 * t4 + t1;
|
||||
const float t9 = t5 * velObsVar;
|
||||
const float t10 = P11 * t7;
|
||||
const float t11 = t1 + t10;
|
||||
const float t12 = P01 * P12;
|
||||
const float t13 = P02 * t4;
|
||||
const float t14 = P01 * P02;
|
||||
const float t15 = P12 * t7;
|
||||
const float t16 = t0 * velObsVar;
|
||||
const float t17 = powf(t2, -2);
|
||||
const float t18 = t4 * velObsVar + t8;
|
||||
const float t19 = t17 * t18;
|
||||
const float t20 = t17 * (t16 + t7 * t8);
|
||||
const float t21 = t0 - t10;
|
||||
const float t22 = t17 * t21;
|
||||
const float t23 = t14 - t15;
|
||||
const float t24 = P01 * t23;
|
||||
const float t25 = t12 - t13;
|
||||
const float t26 = t16 - t21 * t4;
|
||||
const float t27 = t17 * t26;
|
||||
const float t28 = t11 + t7 * velObsVar;
|
||||
const float t30 = t17 * t28;
|
||||
const float t31 = P01 * t25;
|
||||
const float t32 = t23 * t4 + t31;
|
||||
const float t33 = t17 * t32;
|
||||
const float t35 = t24 + t25 * t7;
|
||||
const float t36 = t17 * t35;
|
||||
|
||||
S_det_inverse = t3;
|
||||
|
||||
S_inverse(0, 0) = t3 * t4;
|
||||
S_inverse(0, 1) = t6;
|
||||
S_inverse(1, 1) = t3 * t7;
|
||||
S_inverse(1, 0) = S_inverse(0, 1);
|
||||
|
||||
K(0, 0) = t3 * t8;
|
||||
K(1, 0) = t9;
|
||||
K(2, 0) = t3 * (-t12 + t13);
|
||||
K(0, 1) = t9;
|
||||
K(1, 1) = t11 * t3;
|
||||
K(2, 1) = t3 * (-t14 + t15);
|
||||
|
||||
P_new(0, 0) = P00 - t16 * t19 - t20 * t8;
|
||||
P_new(0, 1) = P01 * (t18 * t22 - t20 * velObsVar + 1);
|
||||
P_new(1, 1) = P11 - t16 * t30 + t22 * t26;
|
||||
P_new(0, 2) = P02 + t19 * t24 + t20 * t25;
|
||||
P_new(1, 2) = P12 + t23 * t27 + t30 * t31;
|
||||
P_new(2, 2) = P22 - t23 * t33 - t25 * t36;
|
||||
P_new(1, 0) = P_new(0, 1);
|
||||
P_new(2, 0) = P_new(0, 2);
|
||||
P_new(2, 1) = P_new(1, 2);
|
||||
}
|
||||
|
||||
void sympyYawEstPrediction(const Vector3f &state, const SquareMatrix3f &P, const Vector2f &d_vel, float d_vel_var,
|
||||
float d_ang_var, SquareMatrix3f &P_new)
|
||||
{
|
||||
const float P00 = P(0, 0);
|
||||
const float P01 = P(0, 1);
|
||||
const float P02 = P(0, 2);
|
||||
const float P11 = P(1, 1);
|
||||
const float P12 = P(1, 2);
|
||||
const float P22 = P(2, 2);
|
||||
const float psi = state(2);
|
||||
const float dvx = d_vel(0);
|
||||
const float dvy = d_vel(1);
|
||||
const float dvxVar = d_vel_var;
|
||||
const float dvyVar = d_vel_var;
|
||||
const float dazVar = d_ang_var;
|
||||
|
||||
const float S0 = cosf(psi);
|
||||
const float S1 = powf(S0, 2.f);
|
||||
const float S2 = sinf(psi);
|
||||
const float S3 = powf(S2, 2.f);
|
||||
const float S4 = S0 * dvy + S2 * dvx;
|
||||
const float S5 = P02 - P22 * S4;
|
||||
const float S6 = S0 * dvx - S2 * dvy;
|
||||
const float S7 = S0 * S2;
|
||||
const float S8 = P01 + S7 * dvxVar - S7 * dvyVar;
|
||||
const float S9 = P12 + P22 * S6;
|
||||
|
||||
P_new(0, 0) = P00 - P02 * S4 + S1 * dvxVar + S3 * dvyVar - S4 * S5;
|
||||
P_new(0, 1) = -P12 * S4 + S5 * S6 + S8;
|
||||
P_new(1, 1) = P11 + P12 * S6 + S1 * dvyVar + S3 * dvxVar + S6 * S9;
|
||||
P_new(0, 2) = S5;
|
||||
P_new(1, 2) = S9;
|
||||
P_new(2, 2) = P22 + dazVar;
|
||||
}
|
||||
|
||||
TEST(YawEstimatorGenerated, SympyVsSymforceUpdate)
|
||||
{
|
||||
const float R = sq(0.1f);
|
||||
|
||||
SquareMatrix<float, 3> P = createRandomCovarianceMatrix3f();
|
||||
|
||||
SquareMatrix<float, 2> innov_var_inv_sympy;
|
||||
float innov_var_det_inv_sympy;
|
||||
SquareMatrix3f P_new_sympy;
|
||||
Matrix<float, 3, 2> K_sympy;
|
||||
sympyYawEstUpdate(P, R, innov_var_inv_sympy, innov_var_det_inv_sympy, K_sympy, P_new_sympy);
|
||||
|
||||
SquareMatrix<float, 2> innov_var_inv_symforce;
|
||||
float innov_var_det_inv_symforce;
|
||||
SquareMatrix3f P_new_symforce;
|
||||
Matrix<float, 3, 2> K_symforce;
|
||||
sym::YawEstComputeMeasurementUpdate(P, R, FLT_EPSILON,
|
||||
&innov_var_inv_symforce,
|
||||
&innov_var_det_inv_symforce, &K_symforce, &P_new_symforce);
|
||||
// copy upper to lower diagonal
|
||||
P_new_symforce(1, 0) = P_new_symforce(0, 1);
|
||||
P_new_symforce(2, 0) = P_new_symforce(0, 2);
|
||||
P_new_symforce(2, 1) = P_new_symforce(1, 2);
|
||||
|
||||
EXPECT_FLOAT_EQ(innov_var_det_inv_sympy, innov_var_det_inv_symforce);
|
||||
EXPECT_TRUE(isEqual(P_new_sympy, P_new_symforce));
|
||||
EXPECT_TRUE(isEqual(K_sympy, K_symforce));
|
||||
EXPECT_TRUE(isEqual(innov_var_inv_sympy, innov_var_inv_symforce));
|
||||
}
|
||||
|
||||
TEST(YawEstimatorGenerated, SympyVsSymforcePrediction)
|
||||
{
|
||||
const float dt = 0.01f;
|
||||
const Vector2f d_vel = Vector2f(0.1f, -0.5f) * dt;
|
||||
const float d_vel_var = sq(2.f * dt);
|
||||
const float d_ang_var = sq(0.1f * dt);
|
||||
|
||||
const Vector3f state(3.f, 10.f, M_PI_F / 2.f);
|
||||
|
||||
SquareMatrix<float, 3> P = createRandomCovarianceMatrix3f();
|
||||
SquareMatrix<float, 3> P_new_symforce;
|
||||
|
||||
sym::YawEstPredictCovariance(state, P, d_vel, d_vel_var, d_ang_var, &P_new_symforce);
|
||||
|
||||
EXPECT_GT(P_new_symforce(0, 0), P(0, 0));
|
||||
EXPECT_GT(P_new_symforce(1, 1), P(1, 1));
|
||||
EXPECT_GT(P_new_symforce(2, 2), P(2, 2));
|
||||
|
||||
SquareMatrix<float, 3> P_new_sympy;
|
||||
sympyYawEstPrediction(state, P, d_vel, d_vel_var, d_ang_var, P_new_sympy);
|
||||
|
||||
EXPECT_TRUE(isEqual(P_new_sympy, P_new_symforce));
|
||||
}
|
||||
Loading…
Reference in New Issue