数据融合>> ukf>> 返回
项目作者： sfwa

项目描述 ：
Unscented Kalman Filter library for state and parameter estimation
高级语言： C++
项目地址: git://github.com/sfwa/ukf.git
创建时间： 2013-08-19T01:30:21Z
项目社区：https://github.com/sfwa/ukf
开源协议：MIT License
下载


UKF

Unscented Kalman filter library. Several different UKF implementations are
provided:

  • Standard Unscented Kalman Filter for state estimation, as originally
    described in [1], with extensions for quaternions as described
    in [2].
  • Square-root Unscented Kalman Filter for state estimation, implemented as
    described in [3].
  • Optimised form of square-root Unscented Kalman filter for parameter
    estimation, implemented as described in [3].

This library makes use of the Eigen library
for linear algebra routines and matrix and vector operations. Heavy use is
made of C++11 and C++14 features in an attempt to avoid any dynamic memory
allocations and maximise opportunities for compile-time optimisations.

A primary goal of the library is to provide efficient UKF implementations for
use on embedded systems, so there is a strong focus on having minimal
dependencies and avoiding non-deterministic operations.

The filter can be compiled using either single or double precision by
choosing one of the following preprocessor definitions:

  • UKF_SINGLE_PRECISION
  • UKF_DOUBLE_PRECISION

Usage

The library contains a number of class templates which are to be specialised
for the particular application. There are three main classes which need to be
specialised to make up an implementation:

  • State vector
  • Measurement vector
  • Core

The following examples are all derived from the unit tests, so have
a look at them for more detail.

State vector

The state vector and measurement vector are made up of a number of fields,
each of which contains a key and a type. The reason for this is to allow
the filter to handle quaternions in the state vector transparently, so
something like the following is allowed:

  1. enum MyFields {
  2.     LatLon,
  3.     Altitude,
  4.     Velocity,
  5.     Attitude
  6. };
  8. using MyStateVector = UKF::StateVector<
  9.     UKF::Field<LatLon, UKF::Vector<2>>,
  10.     UKF::Field<Altitude, real_t>,
  11.     UKF::Field<Velocity, UKF::Vector<3>>,
  12.     UKF::Field<Attitude, UKF::Quaternion>
  13. >;

Internally, these fields are all stored together as one contiguous Eigen
column vector, and all key lookups are done at compile time.

UKF scaling parameters can be adjusted in the following way:

  1. template <> constexpr real_t UKF::Parameters::AlphaSquared<MyStateVector> = 1e-6;

For a description of what the scaling parameters do, see [2] or read
the comments in the code.

Measurement vector

The measurement vector can be specialised in a similar way, but with the
choice of a fixed or dynamic measurement vector:

  1. enum MyFields {
  2.     StaticPressure,
  3.     DynamicPressure,
  4.     Accelerometer,
  5.     Gyroscope
  6. };
  8. using MyMeasurementVector = UKF::FixedMeasurementVector<
  9.     UKF::Field<Accelerometer, UKF::Vector<3>>,
  10.     UKF::Field<Gyroscope, UKF::Vector<3>>,
  11.     UKF::Field<StaticPressure, real_t>,
  12.     UKF::Field<DynamicPressure, real_t>
  13. >;

or:

  1. enum MyFields {
  2.     StaticPressure,
  3.     DynamicPressure,
  4.     Accelerometer,
  5.     Gyroscope
  6. };
  8. using MyMeasurementVector = UKF::DynamicMeasurementVector<
  9.     UKF::Field<Accelerometer, UKF::Vector<3>>,
  10.     UKF::Field<Gyroscope, UKF::Vector<3>>,
  11.     UKF::Field<StaticPressure, real_t>,
  12.     UKF::Field<DynamicPressure, real_t>
  13. >;

For the fixed measurement vector, all measurements have to be provided every
filter iteration. The dynamic measurement vector allows for a filter where
some measurements are not available at every iteration, and so should only be
fused when they are available.

There is a small performance overhead for the dynamic measurement vector,
but it does not do any dynamic memory allocation.

Core

The Core class contains the filter state and step function. It can be
specialised as follows:

  1. using MyCore = UKF::Core<
  2.     MyStateVector,
  3.     MyMeasurementVector,
  4.     UKF::IntegratorRK4
  5. >;

Here, the user-specialised state vector and measurement vector classes are
provided as template parameters, along with the integrator to use for the
process model. For a list of available integrators, see
Integrator.h.

The square-root state estimation filter can be used instead like this:

  1. using MyCore = UKF::SquareRootCore<
  2.     MyStateVector,
  3.     MyMeasurementVector,
  4.     UKF::IntegratorRK4
  5. >;

Specialisations of the process model (for state estimation filters) and
measurement model must also be provided.

Process model

The process model is implemented as an ODE, with a user-provided function to
calculate the derivative. The ODE is then solved using the integrator method
specified in the Core class specialisation.

Here is an example of a process model for a simple state vector:

  1. using ProcessModelTestStateVector = UKF::StateVector<
  2.     UKF::Field<Position, UKF::Vector<3>>,
  3.     UKF::Field<Velocity, UKF::Vector<3>>
  4. >;
  6. template <> template <>
  7. ProcessModelTestStateVector ProcessModelTestStateVector::derivative<>() const {
  8.     ProcessModelTestStateVector temp;
  9.     /* Position derivative. */
  10.     temp.set_field<Position>(get_field<Velocity>());
  12.     /* Velocity derivative. */
  13.     temp.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));
  15.     return temp;
  16. }

Also, the process model can take an arbitrary number of user-specified
inputs, like this:

  1. template <> template <>
  2. ProcessModelTestStateVector ProcessModelTestStateVector::derivative<UKF::Vector<3>>(
  3.         const UKF::Vector<3>& acceleration) const {
  4.     ProcessModelTestStateVector temp;
  5.     /* Position derivative. */
  6.     temp.set_field<Position>(get_field<Velocity>());
  8.     /* Velocity derivative. */
  9.     temp.set_field<Velocity>(acceleration);
  11.     return temp;
  12. }

Measurement model

The measurement model is specified per field, in order to allow the expected
measurement vector to be constructed for the dynamic measurement vector where
not all measurements may be available each iteration. Each measurement model
function takes a state vector as an input.

The state vector type is provided to the measurement model specialisation as
a template parameter; this allows a measurement vector class to be shared
across multiple state vectors, with difference process model defined for
each.

Here is an example of a measurement model for a simple measurement vector and
state vector:

  1. using MyMeasurementVector = UKF::DynamicMeasurementVector<
  2.     UKF::Field<StaticPressure, real_t>,
  3.     UKF::Field<DynamicPressure, real_t>,
  4.     UKF::Field<Accelerometer, UKF::Vector<3>>,
  5.     UKF::Field<Gyroscope, UKF::Vector<3>>
  6. >;
  8. using MyStateVector = UKF::StateVector<
  9.     UKF::Field<Velocity, UKF::Vector<3>>,
  10.     UKF::Field<AngularVelocity, UKF::Vector<3>>,
  11.     UKF::Field<Attitude, UKF::Quaternion>,
  12.     UKF::Field<Altitude, real_t>
  13. >;
  15. template <> template <>
  16. UKF::Vector<3> MyMeasurementVector::expected_measurement
  17. <MyStateVector, Accelerometer>(const MyStateVector& state) {
  18.     return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8);
  19. }
  21. template <> template <>
  22. UKF::Vector<3> MyMeasurementVector::expected_measurement
  23. <MyStateVector, Gyroscope>(const MyStateVector& state) {
  24.     return state.get_field<AngularVelocity>();
  25. }
  27. template <> template <>
  28. real_t MyMeasurementVector::expected_measurement
  29. <MyStateVector, StaticPressure>(const MyStateVector& state) {
  30.     return 101.3 - 1.2*(state.get_field<Altitude>() / 100.0);
  31. }
  33. template <> template <>
  34. real_t MyMeasurementVector::expected_measurement
  35. <MyStateVector, DynamicPressure>(const MyStateVector& state) {
  36.     return 0.5 * 1.225 * state.get_field<Velocity>().squaredNorm();
  37. }

As with the process model, the measurement model can take an arbitrary number
of user-specified inputs:

  1. template <> template <>
  2. UKF::Vector<3> MyMeasurementVector::expected_measurement
  3. <MyStateVector, Accelerometer, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
  4.     return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8) + input;
  5. }
  7. template <> template <>
  8. UKF::Vector<3> MyMeasurementVector::expected_measurement
  9. <MyStateVector, Gyroscope, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
  10.     return state.get_field<AngularVelocity>();
  11. }
  13. template <> template <>
  14. real_t MyMeasurementVector::expected_measurement
  15. <MyStateVector, StaticPressure, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
  16.     return 101.3 - 1.2*(state.get_field<Altitude>() / 100.0);
  17. }
  19. template <> template <>
  20. real_t MyMeasurementVector::expected_measurement
  21. <MyStateVector, DynamicPressure, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
  22.     return 0.5 * 1.225 * state.get_field<Velocity>().squaredNorm();
  23. }

Initialisation

The filter state, covariance, process noise covariance and measurement noise
covariance should be initialised to appropriate values, e.g.:

  1. MyCore test_filter;
  2. test_filter.state.set_field<Position>(UKF::Vector<3>(0, 0, 0));
  3. test_filter.state.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));
  4. test_filter.state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
  5. test_filter.state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));
  6. test_filter.covariance = MyStateVector::CovarianceMatrix::Zero();
  7. test_filter.covariance.diagonal() << 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10;
  8. test_filter.process_noise_covariance = MyStateVector::CovarianceMatrix::Identity()*1e-5;
  9. test_filter.measurement_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05;

Or, for the SR-UKF:

  1. MyCore test_filter;
  2. test_filter.state.set_field<Position>(UKF::Vector<3>(0, 0, 0));
  3. test_filter.state.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));
  4. test_filter.state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
  5. test_filter.state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));
  6. test_filter.root_covariance = MyStateVector::CovarianceMatrix::Zero();
  7. test_filter.root_covariance.diagonal() << 100, 100, 100, 10, 10, 10, 1, 1, 2.2, 3.2, 3.2, 3.2;
  8. test_filter.process_noise_root_covariance = MyStateVector::CovarianceMatrix::Identity()*3e-2;
  9. test_filter.measurement_root_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05;
  10. test_filter.measurement_root_covariance = test_filter.measurement_root_covariance.array().sqrt();

Currently, only a diagonal measurement noise covariance matrix is supported.

Iteration

The general steps for carrying out a filter iteration are something like:

  1. MyMeasurementVector meas;
  2. meas.set_field<Accelerometer>(UKF::Vector<3>(0.0, 0.0, 9.8));
  3. meas.set_field<Gyroscope>(UKF::Vector<3>(0.0, 0.0, 0.0));
  4. meas.set_field<StaticPressure>(101300.0);
  5. meas.set_field<DynamicPressure>(101300.0);
  7. test_filter.step(0.01, meas);

Or, if it’s necessary to do things with the internal filter state (e.g.
filter health monitoring), then iteration can be split up into three steps:

  1. MyMeasurementVector meas;
  2. meas.set_field<Accelerometer>(UKF::Vector<3>(0.0, 0.0, 9.8));
  3. meas.set_field<Gyroscope>(UKF::Vector<3>(0.0, 0.0, 0.0));
  4. meas.set_field<StaticPressure>(101300.0);
  5. meas.set_field<DynamicPressure>(101300.0);
  7. test_filter.a_priori_step(0.01);
  8. /*
  9. At this point, the state and covariance (or root_covariance) variables
  10. reflect the a priori state and (root_)covariance.
  11. */
  12. test_filter.innovation_step(meas);
  13. /* Innovation and innovation_(root_)covariance variables are now set. */
  14. test_filter.a_posteriori_step();
  15. /* State and (root_)covariance variables are set to the a priori values.

Building test and benchmark suites

Build requires a compiler supporting C++17.

First, clone the repo:

  1. git clone git@github.com:sfwa/ukf.git && cd ukf

Then, configure CMake:

  1. cmake -B ./build -DCMAKE_BUILD_TYPE=Release

Then build:

  1. cmake --build ./build --config Release

Build and run the unit test suite:

  1. cd build && make unittest && test/unittest && cd ..

If desired, build and run the benchmark suite:

  1. cd build && make benchmark && benchmark/benchmark && cd ..

Examples

Some examples are provided here.

References

[1] “A New Extension of the Kalman Filter to Nonlinear Systems:,
S. J. Julier and J. K. Uhlmann,
https://www.cs.unc.edu/~welch/kalman/media/pdf/Julier1997_SPIE_KF.pdf

[2] “Unscented Filtering for Spacecraft Attitude Estimation”, John L.
Crassidis and F. Landis Markley, http://www.acsu.buffalo.edu/~johnc/uf_att.pdf

[3] “The Square-Root Unscented Kalman Filter for State and Parameter-Estimation”,
Rudolph van der Merwe and Eric A. Wan,
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.80.1421&rep=rep1&type=pdf