Skip to content

Commit

Permalink
💄
Browse files Browse the repository at this point in the history
  • Loading branch information
wumanu committed Sep 10, 2015
1 parent 00cd269 commit 4fcaa7d
Showing 1 changed file with 3 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -184,7 +184,7 @@ class MultiSensorSigmaPointUpdatePolizzle<

model.id(i);

quadrature.propergate_points(h, p_X, p_Q, p_Y);
quadrature.propagate_points(h, p_X, p_Q, p_Y);

auto mu_y = p_Y.center();

Expand Down Expand Up @@ -214,7 +214,7 @@ class MultiSensorSigmaPointUpdatePolizzle<
return feature;
};
PointSet<LocalObsrv, NumberOfPoints> p_Y_body;
quadrature.propergate_points(h_body, p_X, p_Q_partial, p_Y_body);
quadrature.propagate_points(h_body, p_X, p_Q_partial, p_Y_body);
Eigen::Vector3d mu_y_body = p_Y_body.mean();

valid = true;
Expand All @@ -241,7 +241,7 @@ class MultiSensorSigmaPointUpdatePolizzle<
model.embedded_obsrv_model().tail_model().observation(x, w));
};
PointSet<LocalObsrv, NumberOfPoints> p_Y_tail;
quadrature.propergate_points(h_tail, p_X, p_Q_partial, p_Y_tail);
quadrature.propagate_points(h_tail, p_X, p_Q_partial, p_Y_tail);
Eigen::Vector3d mu_y_tail = p_Y_tail.mean();
Eigen::Matrix<double, 3, NumberOfPoints> Y_tail = p_Y_tail.centered_points();
Eigen::Matrix<double, 3, 3> c_yy_tail = (Y_tail * W.asDiagonal() * Y_tail.transpose()).eval();
Expand Down

0 comments on commit 4fcaa7d

Please sign in to comment.