BFL filter update causes seg fault

[aruisdante]

Currently calling the update method in LinearFilter will cause a seg fault when the actual call to BFL's EKF update method is called. The A,B,H and z matrices all seem to be properly formed. Needs more exploration.

[aruisdante]

@mitchellweb1 I've assigned you as well so that we can have two heads going against this, and so you can learn BFL to help out in the aero packages as well.

[mitchellwills]

This is the best I have so far. The last BFL call is BFL::LinearAnalyticConditionalGaussian::ExpectedValueGet but the stack trace isn't all that helpful. I'll probably go though the tutorials later and see if I can learn anything

#0  apply const>, boost::numeric::ublas::vector_reference const> > (i=0, e2=..., e1=...) at /usr/include/boost/numeric/ublas/functional.hpp:831
#1  operator() (i=0, this=) at /usr/include/boost/numeric/ublas/matrix_expression.hpp:3452
#2  indexing_vector_assign, boost::numeric::ublas::matrix_vector_binary1, boost::numeric::ublas::vector, boost::numeric::ublas::matrix_vector_prod1, boost::numeric::ublas::vector, double> > > (e=..., v=...) at /usr/include/boost/numeric/ublas/detail/vector_assign.hpp:265
#3  vector_assign, boost::numeric::ublas::matrix_vector_binary1, boost::numeric::ublas::vector, boost::numeric::ublas::matrix_vector_prod1, boost::numeric::ublas::vector, double> > > (e=..., v=...) at /usr/include/boost/numeric/ublas/detail/vector_assign.hpp:277
#4  vector_assign, boost::numeric::ublas::matrix_vector_binary1, boost::numeric::ublas::vector, boost::numeric::ublas::matrix_vector_prod1, boost::numeric::ublas::vector, double> > > (v=..., e=...) at /usr/include/boost/numeric/ublas/detail/vector_assign.hpp:448
#5  vector, boost::numeric::ublas::vector, boost::numeric::ublas::matrix_vector_prod1, boost::numeric::ublas::vector, double> > > (ae=..., this=0x7fffffffcf80)
    at /usr/include/boost/numeric/ublas/vector.hpp:122
#6  MatrixWrapper::Matrix::operator* (this=, b=...)
    at /tmp/buildd/ros-groovy-bfl-0.1.0/debian/ros-groovy-bfl/opt/ros/groovy/stacks/bfl/build/bfl-tar/src/wrappers/matrix/matrix_BOOST.cpp:164
#7  0x00007ffff762c2ee in BFL::LinearAnalyticConditionalGaussian::ExpectedValueGet (this=0x627b50)
    at /tmp/buildd/ros-groovy-bfl-0.1.0/debian/ros-groovy-bfl/opt/ros/groovy/stacks/bfl/build/bfl-tar/src/pdf/linearanalyticconditionalgaussian.cpp:75
#8  0x00007ffff7631251 in BFL::AnalyticSystemModelGaussianUncertainty::PredictionGet (this=0x625ac0, u=..., x=...)
    at /tmp/buildd/ros-groovy-bfl-0.1.0/debian/ros-groovy-bfl/opt/ros/groovy/stacks/bfl/build/bfl-tar/src/model/analyticsystemmodel_gaussianuncertainty.cpp:60
#9  0x00007ffff7637088 in BFL::ExtendedKalmanFilter::SysUpdate (this=0x628cc0, sysmodel=0x625ac0, u=...)
    at /tmp/buildd/ros-groovy-bfl-0.1.0/debian/ros-groovy-bfl/opt/ros/groovy/stacks/bfl/build/bfl-tar/src/filter/extendedkalmanfilter.cpp:79
#10 0x00007ffff76345ff in BFL::KalmanFilter::UpdateInternal (this=0x628cc0, sysmodel=, u=..., measmodel=0x625a40, z=..., s=...)
    at /tmp/buildd/ros-groovy-bfl-0.1.0/debian/ros-groovy-bfl/opt/ros/groovy/stacks/bfl/build/bfl-tar/src/filter/kalmanfilter.cpp:137
#11 0x00007ffff7633eaf in BFL::Filter::Update (this=0x628cc0, sysmodel=0x625ac0, measmodel=0x625a40, z=...)
    at /tmp/buildd/ros-groovy-bfl-0.1.0/debian/ros-groovy-bfl/opt/ros/groovy/stacks/bfl/build/bfl-tar/src/filter/filter.cpp:80
#12 0x000000000040a8f1 in kvh_driver::LinearFilter::update (this=0x626d40, input=..., measurement=...)
    at /home/mitchell/srr/src/kvh/kvh_driver/src/helpers/linearfilter.cpp:94
#13 0x000000000040739b in kvh_driver::KVHDriverNode::update (this=0x7fffffffdc30, event=...) at /home/mitchell/srr/src/kvh/kvh_driver/src/nodes/kvh_driver_node.cpp:99
#14 0x00007ffff7b1e5e4 in ros::TimerManager::TimerQueueCallback::call() () from /opt/ros/groovy/lib/libroscpp.so
#15 0x00007ffff7b389b9 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/groovy/lib/libroscpp.so
#16 0x00007ffff7b3a45b in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/groovy/lib/libroscpp.so
#17 0x00007ffff7b89908 in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) () from /opt/ros/groovy/lib/libroscpp.so
#18 0x00007ffff7b6e8db in ros::spin() () from /opt/ros/groovy/lib/libroscpp.so
#19 0x000000000040611a in main (argc=1, argv=0x7fffffffdef8) at /home/mitchell/srr/src/kvh/kvh_driver/src/nodes/kvh_driver_node.cpp:170

[aruisdante]

So I think there's a tag to wrap code comments in so that Github doesn't try and parse them (and so they show up code-y). Because you just linked this to about 7 issues due to the #xxx's in there. Yeah, see this: http://www.arlocarreon.com/blog/github/pre-and-code-tags-in-github-markdown/

[aruisdante]

Updated your comment to use the code tags.

[aruisdante]

So just looking at that trace callback, it looks like it could be a matrix dimension mismatch in multiplication. You'd think they would have put a more descriptive error message in there for such a thing, but it's possible they didn't for performance reasons (I know this is the case with their element accessor overloads, if you try and access Matrix(0,0) it seg falts because the indicies start at 1).

[aruisdante]

So I think it might be the fact that the system has no inputs (and thus shouldn't have a B matrix), but I'm building a 6x1 B matrix of all zeros. I'll test this theory later today once I get into lab. If that's the case, we'll have to alter the way the constructor works so that the AB vector isn't built by default, instead we store separate A and B matrices, and then depending on the values of statesize, inputsize, and measurementsize, build the appropriately parameterized system and measurement models.

[aruisdante]

Yep, this is absolutely the problem. I tried it with just straight passing the A-matrix, and it worked fine. So we need to refactor the code to contextually build the system/measurement model dependent on the given state/input/measurement size parameters.