Replaced cwiseProduct() with array().square(). Killed deprecated comments.
parent
22c3af906e
commit
caa45ad67b
|
@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
|
|||
list<boost::tuple<Vector, double, double> > results;
|
||||
|
||||
Vector pseudo(m); // allocate storage for pseudo-inverse
|
||||
Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); // calculate weights once
|
||||
Vector weights = reciprocal(sigmas.array().square()); // calculate weights once
|
||||
|
||||
// We loop over all columns, because the columns that can be eliminated
|
||||
// are not necessarily contiguous. For each one, estimate the corresponding
|
||||
|
|
|
@ -213,12 +213,6 @@ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
|
|||
GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/**
|
||||
* elementwise multiplication
|
||||
* @param a first vector
|
||||
* @param b second vector
|
||||
* @return vector [a(i)*b(i)]
|
||||
*/
|
||||
GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b);
|
||||
#endif
|
||||
|
||||
|
|
|
@ -198,7 +198,7 @@ TEST(Vector, weightedPseudoinverse )
|
|||
// create sigmas
|
||||
Vector sigmas(2);
|
||||
sigmas(0) = 0.1; sigmas(1) = 0.2;
|
||||
Vector weights = reciprocal(sigmas.cwiseProduct(sigmas));
|
||||
Vector weights = reciprocal(sigmas.array().square());
|
||||
|
||||
// perform solve
|
||||
Vector actual; double precision;
|
||||
|
@ -224,7 +224,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
|
|||
// create sigmas
|
||||
Vector sigmas(2);
|
||||
sigmas(0) = 0.0; sigmas(1) = 0.2;
|
||||
Vector weights = reciprocal(sigmas.cwiseProduct(sigmas));
|
||||
Vector weights = reciprocal(sigmas.array().square());
|
||||
// perform solve
|
||||
Vector actual; double precision;
|
||||
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
|
||||
|
@ -243,7 +243,7 @@ TEST(Vector, weightedPseudoinverse_nan )
|
|||
{
|
||||
Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
|
||||
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
|
||||
Vector weights = reciprocal(sigmas.cwiseProduct(sigmas));
|
||||
Vector weights = reciprocal(sigmas.array().square());
|
||||
Vector pseudo; double precision;
|
||||
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
|
||||
|
||||
|
|
|
@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
|
|||
EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));
|
||||
|
||||
// do the above update again, this time with a full Matrix Q
|
||||
Matrix modelQ = diag(sigmas.cwiseProduct(sigmas));
|
||||
Matrix modelQ = diag(sigmas.array().square());
|
||||
KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ);
|
||||
KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ);
|
||||
|
||||
|
|
|
@ -186,8 +186,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
|
|||
Matrix b_g = bRn * n_g_cross_;
|
||||
H = collect(3, &b_g, &Z_3x3, &I_3x3);
|
||||
// And the measurement noise, TODO: should be created once where sigmas_v_a is given
|
||||
R = diag(sigmas_v_a_.cwiseQuotient(sigmas_v_a_));
|
||||
|
||||
R = diag(sigmas_v_a_.array().square());
|
||||
}
|
||||
|
||||
// update the Kalman filter
|
||||
|
|
Loading…
Reference in New Issue