From 7d0b5629087e6c744f58c4e587cca1b72196255b Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 5 Mar 2016 21:20:32 -0500 Subject: [PATCH] Fixed two missed replacements of cwiseProduct() with array().square(). --- gtsam_unstable/slam/AHRS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 511514347..258bc5d03 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -48,7 +48,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, Vector3 var_omega_w = 0 * ones(3); // TODO Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); Vector3 var_omega_a = (0.034 * 0.034) * ones(3); - Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g); + Vector3 sigmas_v_g_sq = sigmas_v_g.array().square(); var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding @@ -171,7 +171,7 @@ std::pair AHRS::aid( // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; H = collect(3, &n_g_cross_, &Z_3x3, &bRn); - R = trans(bRn) * diag(sigmas_v_a_.cwiseProduct(sigmas_v_a_)) * bRn; + R = trans(bRn) * diag(sigmas_v_a_.array().square()) * bRn; } else { // my measurement prediction (in body frame): // F(:,k) = bias - b_g