Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Making DKF compatible with dynamically-sized system models #54

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Making DKF compatible with dynamically-sized system models #54

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions include/mrs_lib/dkf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ namespace mrs_lib
* \param line_variance Variance defining the uncertainty of the measured state in the direction perpendicular to the measurement line. The uncertainty in the parallel direciton is assumed to be infinite for this case of DKF.
* \return The state and covariance after the correction update.
*/
virtual std::enable_if_t<(n > 3), statecov_t> correctLine(const statecov_t& sc, const pt3_t& line_origin, const vec3_t& line_direction, const double line_variance) const
virtual std::enable_if_t<((n > 3) || (n == Eigen::Dynamic)), statecov_t> correctLine(const statecov_t& sc, const pt3_t& line_origin, const vec3_t& line_direction, const double line_variance) const
{
assert(line_direction.norm() > 0.0);

Expand All @@ -96,7 +96,7 @@ namespace mrs_lib
using o_t = Eigen::Matrix<double, 3, 1>;
using R_t = Eigen::Matrix<double, 2, 2>;

const M_t M = M_t::Identity();
const M_t M = M_t::Identity(3, sc.P.cols());
const W_t W = line_direction;
const o_t o = line_origin;

Expand Down Expand Up @@ -130,7 +130,7 @@ namespace mrs_lib
* \param plane_variance Variance defining the uncertainty of the measured state in the direction perpendicular to the measurement plane. The uncertainty in the span of the plane is assumed to be infinite for this case of DKF.
* \return The state and covariance after the correction update.
*/
virtual std::enable_if_t<(n > 3), statecov_t> correctPlane(const statecov_t& sc, const pt3_t& plane_origin, const vec3_t& plane_normal, const double plane_variance) const
virtual std::enable_if_t<((n > 3) || (n == Eigen::Dynamic)), statecov_t> correctPlane(const statecov_t& sc, const pt3_t& plane_origin, const vec3_t& plane_normal, const double plane_variance) const
{
assert(plane_normal.norm() > 0.0);

Expand All @@ -140,7 +140,7 @@ namespace mrs_lib
using o_t = Eigen::Matrix<double, 3, 1>;
using R_t = Eigen::Matrix<double, 1, 1>;

const M_t M = M_t::Identity();
const M_t M = M_t::Identity(3, sc.P.cols());
const o_t o = plane_origin;

const N_t N = plane_normal.normalized(); //works for plane
Expand Down
Loading