Skip to content

D
dlib
Commit 4e01b779 authored by Davis King's avatar Davis King
Browse files
Options

Cleanup

parent 4ec6b1f0
Hide whitespace changes
Inline Side-by-side
Showing with 169 additions and 20 deletions
dlib/filtering/kalman_filter.cpp
View file @ 4e01b779
......@@ -36,7 +36,7 @@ namespace dlib
if (i > 0)
{
// the filter should fit the data
rs.add(std::abs(vals[i]-filt.get_predicted_next_state()));
rs.add(std::abs(vals[i]-filt.get_predicted_next_position()));
}
double next_filt = filt(vals[i]);
if (i > 0)
......
dlib/filtering/kalman_filter.h
View file @ 4e01b779
......@@ -172,10 +172,15 @@ namespace dlib
double meas_noise,
double acc,
double max_meas_dev
) : measurement_noise(meas_noise),
) :
measurement_noise(meas_noise),
typical_acceleration(acc),
max_measurement_deviation(max_meas_dev)
{
DLIB_CASSERT(meas_noise >= 0);
DLIB_CASSERT(acc >= 0);
DLIB_CASSERT(max_meas_dev >= 0);
kal.set_observation_model({1, 0});
kal.set_transition_model( {1, 1,
0, 1});
......@@ -201,35 +206,35 @@ namespace dlib
*this = momentum_filter(measurement_noise, typical_acceleration, max_measurement_deviation);
}
double get_predicted_next_state(
double get_predicted_next_position(
) const
{
return kal.get_predicted_next_state()(0);
}
double operator()(
const double val
const double measured_position
)
{
auto x = kal.get_predicted_next_state();
const auto max_deviation = max_measurement_deviation*measurement_noise;
// Check if val has suddenly jumped in value by a whole lot. This could happen if
// the velocity term experiences a much larger than normal acceleration, e.g.
// because the underlying object is doing a maneuver. If this happens then we
// clamp the state so that the predicted next value is no more than
// max_deviation away from val at all times.
if (x(0) > val + max_deviation)
// Check if measured_position has suddenly jumped in value by a whole lot. This
// could happen if the velocity term experiences a much larger than normal
// acceleration, e.g. because the underlying object is doing a maneuver. If
// this happens then we clamp the state so that the predicted next value is no
// more than max_deviation away from measured_position at all times.
if (x(0) > measured_position + max_deviation)
{
x(0) = val + max_deviation;
x(0) = measured_position + max_deviation;
kal.set_state(x);
}
else if (x(0) < val - max_deviation)
else if (x(0) < measured_position - max_deviation)
{
x(0) = val - max_deviation;
x(0) = measured_position - max_deviation;
kal.set_state(x);
}
kal.update({val});
kal.update({measured_position});
return kal.get_current_state()(0);
}
......
dlib/filtering/kalman_filter_abstract.h
View file @ 4e01b779
......@@ -218,41 +218,122 @@ namespace dlib
{
/*!
WHAT THIS OBJECT REPRESENTS
This object is a simple tool for filtering a single scalar value that
measures the location of a moving object that has some non-trivial
momentum. Importantly, the measurements are noisy and the object can
experience sudden unpredictable accelerations. To accomplish this
filtering we use a simple Kalman filter with a state transition model of:
position_{i+1} = position_{i} + velocity_{i}
velocity_{i+1} = velocity_{i} + some_unpredictable_acceleration
and a measurement model of:
measured_position_{i} = position_{i} + measurement_noise
Where some_unpredictable_acceleration and measurement_noise are 0 mean Gaussian
noise sources with standard deviations of get_typical_acceleration() and
get_measurement_noise() respectively.
To allow for really sudden and large but infrequent accelerations, at each
step we check if the current measured position deviates from the predicted
filtered position by more than get_max_measurement_deviation()*get_measurement_noise()
and if so we adjust the filter's state to keep it within these bounds.
This allows the moving object to undergo large unmodeled accelerations, far
in excess of what would be suggested by get_typical_acceleration(), without
then experiencing a long lag time where the Kalman filter has to "catches
up" to the new position.
!*/
public:
momentum_filter(
) = default;
/*!
ensures
- #get_measurement_noise() == 2
- #get_typical_acceleration() == 0.1
- #get_max_measurement_deviation() == 3
!*/
momentum_filter(
double meas_noise,
double acc,
double max_meas_dev
);
/*!
requires
- meas_noise >= 0
- acc >= 0
- max_meas_dev >= 0
ensures
- #get_measurement_noise() == meas_noise
- #get_typical_acceleration() == acc
- #get_max_measurement_deviation() == max_meas_dev
!*/
momentum_filter() = default;
double get_measurement_noise (
) const;
/*!
ensures
- Returns the standard deviation of the 0 mean Gaussian noise that corrupts
measurements of the moving object.
!*/
double get_typical_acceleration (
) const;
/*!
ensures
- We assume that the moving object experiences random accelerations that
are distributed by 0 mean Gaussian noise with get_typical_acceleration()
standard deviation.
!*/
double get_max_measurement_deviation (
) const;
/*!
ensures
- This object will never let the filtered location of the object deviate
from the measured location by much more than
get_max_measurement_deviation()*get_measurement_noise().
!*/
void reset(
);
double get_predicted_next_state(
) const;
/*!
ensures
- Returns this object to the state immediately after construction. To be precise, we do:
*this = momentum_filter(get_measurement_noise(), get_typical_acceleration(), get_max_measurement_deviation());
!*/
double operator()(
const double val
const double measured_position
);
/*!
ensures
- Updates the Kalman filter with the new measured position of the object
and returns the new filtered estimate of the object's position, now that
we have seen the latest measured position.
- #get_predicted_next_position() == the prediction for the *next* place we
will see the object. That is, where we think it will be in the future
rather than where it is now.
!*/
double get_predicted_next_position (
) const;
/*!
ensures
- Returns the Kalman filter's estimate of the next position we will see the object.
!*/
};
std::ostream& operator << (std::ostream& out, const momentum_filter& item);
void serialize(const momentum_filter& item, std::ostream& out);
void deserialize(momentum_filter& item, std::istream& in);
/*!
Provide printing and serialization support.
!*/
// ----------------------------------------------------------------------------------------
......@@ -265,6 +346,19 @@ namespace dlib
- sequences.size() != 0
- for all valid i: sequences[i].size() > 4
- smoothness >= 0
ensures
- This function finds the "optimal" settings of a momentum_filter based on
recorded measurement data stored in sequences. Here we assume that each
vector in sequences is a complete track history of some object's measured
positions. What we do is find the momentum_filter that minimizes the
following objective function:
sum of abs(predicted_location[i] - measured_location[i]) + smoothness*abs(filtered_location[i]-filtered_location[i-1])
Where i is a time index.
The sum runs over all the data in sequences. So what we do is find the
filter settings that produce smooth filtered trajectories but also produce
filtered outputs that are as close to the measured positions as possible.
The larger the value of smoothness the less jittery the filter outputs will
be, but they might become biased or laggy if smoothness is set really high.
!*/
// ----------------------------------------------------------------------------------------
......@@ -285,20 +379,58 @@ namespace dlib
class rect_filter
{
/*!
WHAT THIS OBJECT REPRESENTS
This object simply contains four momentum_filters and applies them to the
4 components of a dlib::rectangle's position. It therefore allows you to
easily filter a sequence of rectangles. For instance, it can be used to
smooth the output of an object detector running on a video.
!*/
public:
rect_filter() = default;
rect_filter(
) = default;
/*!
ensures
- The four momentum_filters in this object are default initialized.
!*/
rect_filter(
const momentum_filter& filt
);
/*!
ensures
- #get_left() == filt
- #get_top() == filt
- #get_right() == filt
- #get_bottom() == filt
!*/
drectangle operator()(
const drectangle& r
);
/*!
ensures
- Runs the given rectangle through the momentum_filters and returns the
filtered rectangle location. That is, performs:
return drectangle(get_left()(r.left()),
get_top()(r.top()),
get_right()(r.right()),
get_bottom()(r.bottom()));
!*/
drectangle operator()(
const rectangle& r
);
/*!
ensures
- Runs the given rectangle through the momentum_filters and returns the
filtered rectangle location. That is, performs:
return drectangle(get_left()(r.left()),
get_top()(r.top()),
get_right()(r.right()),
get_bottom()(r.bottom()));
!*/
const momentum_filter& get_left() const;
momentum_filter& get_left();
......@@ -308,10 +440,16 @@ namespace dlib
momentum_filter& get_right();
const momentum_filter& get_bottom() const;
momentum_filter& get_bottom();
/*!
Provides access to the 4 momentum_filters used to filter the 4 coordinates that define a rectangle.
!*/
};
void serialize(const rect_filter& item, std::ostream& out);
void deserialize(rect_filter& item, std::istream& in);
/*!
Provide serialization support.
!*/
// ----------------------------------------------------------------------------------------
......@@ -323,6 +461,12 @@ namespace dlib
requires
- rects.size() > 4
- smoothness >= 0
ensures
- This routine simply invokes find_optimal_momentum_filter() to find the
momentum_filter that works best on the provided sequence of rectangles. It
then constructs a rect_filter using that momentum_filter and returns it.
Therefore, this routine finds the rect_filter that is "optimal" for filtering
the given sequence of rectangles.
!*/
// ----------------------------------------------------------------------------------------
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment