Age | Commit message (Collapse) | Author |
|
The Jacobian of the measurement model, Hx and Hy were updated using code
generated with matlab's ccode() function. This generates a number of
intermediate values, which are an optimization by the function's
description, before setting the final values. In any case, it's not
necessarily more readable, but it does eliminate the incredbily long
lines used previously.
|
|
|
|
|
|
The changes add quaternion estimation to the body state. The quaternion is still
provided as an input, but it is ignored.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
1-Pt RANSAC is a method for detecting outlier measurements in the EKF
framework. This algorithm is as described in Civera 2010.
|
|
|
|
This is sort of a fudge to compensate for poor camera to IMU
calibration. It injects additional yaw into the Rc2b matrices.
|
|
|
|
This is not tested to be working yet.
|
|
|
|
|
|
|
|
|
|
But they don't really work.
|
|
It makes sense to decouple the measurement type, referred to as feature
type or fType from the Feature class since a feature can be measured in
different ways throughout its lifetime. Better to keep knowledge of the
measurement type in the State class.
|
|
Feature methods were updated to accomodate both types of features.
|
|
|
|
|
|
|
|
|
|
() back to main
|
|
|
|
|
|
|
|
Update was too vague since a lot of things get updated. dx more clearly refers
to the incremental update after a Kalman update.
|
|
|
|
|
|
|
|
|
|
Still needs to be debugged as the result is way off, but it is all
there.
|
|
|
|
|
|
|
|
|
|
State::kalmanUpdate should be general enough to work with both the full
and independent update methods.
The State::innovation method calculates the innovation vector directly
rather than building z and hhat individually.
|
|
|
|
Method to compute R for given vector of z measurements. Added a method
State::Hrows(z) to return number of rows in H(z).
|
|
|
|
Added a method to return an H matrix for the provided measurements. This
should be a versatile method that can work for the full H update or the
indpendent update depending on what measurements are passed.
Some helper methods were also added for determining a features
location in the state vector and for return a feature pointer by id.
|
|
This only corresponds foo.cpp to foo.h, nothing more than that.
|
|
Testing shows that this is not, in fact, faster. Possibly it can be made
more efficient in the future.
|
|
Added a boolean function that returns if the feature is in the FOV. This
method does not use the camera calibration, it just tests if the
feature's angle w.r.t. the body.
|
|
After updating the state according to the motion model we check if
features are still in the field of view. If they are not they are
cleanly removed. Removal requires deletion of the feature, removal from
the features list and shrinkng the P matrix.
|
|
|