ukf
所属分类:其他
开发工具:C++
文件大小:0KB
下载次数:0
上传日期:2022-06-17 12:51:25
上 传 者:
sh-1993
说明: 用于状态和参数估计的无中心卡尔曼滤波器库,
(Unscented Kalman Filter library for state and parameter estimation,)
文件列表:
CMakeLists.txt (1554, 2022-06-17)
LICENSE (1059, 2022-06-17)
benchmark/ (0, 2022-06-17)
benchmark/BenchmarkMain.cpp (52, 2022-06-17)
benchmark/CMakeLists.txt (1325, 2022-06-17)
benchmark/CoreBenchmark.cpp (6720, 2022-06-17)
benchmark/MeasurementVectorBenchmark.cpp (11168, 2022-06-17)
benchmark/SquareRootCoreBenchmark.cpp (7502, 2022-06-17)
benchmark/StateVectorBenchmark.cpp (6240, 2022-06-17)
examples/ (0, 2022-06-17)
examples/CMakeLists.txt (467, 2022-06-17)
examples/ahrs/ (0, 2022-06-17)
examples/ahrs/CMakeLists.txt (338, 2022-06-17)
examples/ahrs/ahrs.cpp (18139, 2022-06-17)
examples/ahrs/ahrs.h (4137, 2022-06-17)
examples/ahrs/python/ (0, 2022-06-17)
examples/ahrs/python/ukf/ (0, 2022-06-17)
examples/ahrs/python/ukf/__init__.py (8581, 2022-06-17)
examples/sfwa_ukf/ (0, 2022-06-17)
examples/sfwa_ukf/CMakeLists.txt (350, 2022-06-17)
examples/sfwa_ukf/cukf.cpp (19029, 2022-06-17)
examples/sfwa_ukf/cukf.h (6171, 2022-06-17)
examples/sfwa_ukf/python/ (0, 2022-06-17)
examples/sfwa_ukf/python/ukf/ (0, 2022-06-17)
examples/sfwa_ukf/python/ukf/__init__.py (13506, 2022-06-17)
include/ (0, 2022-06-17)
include/UKF/ (0, 2022-06-17)
include/UKF/Config.h (287, 2022-06-17)
include/UKF/Core.h (27443, 2022-06-17)
include/UKF/Integrator.h (2374, 2022-06-17)
include/UKF/MeasurementVector.h (38652, 2022-06-17)
include/UKF/StateVector.h (26660, 2022-06-17)
include/UKF/Types.h (1889, 2022-06-17)
patch/ (0, 2022-06-17)
patch/eigen-3.2.8-no-malloc-in-llt.patch (703, 2022-06-17)
... ...
# UKF
Unscented Kalman filter library. Several different UKF implementations are
provided:
* Standard Unscented Kalman Filter for state estimation, as originally
described in [[1]](https://github.com/sfwa/ukf/blob/master/#ref1), with extensions for quaternions as described
in [[2]](https://github.com/sfwa/ukf/blob/master/#ref2).
* Square-root Unscented Kalman Filter for state estimation, implemented as
described in [[3]](https://github.com/sfwa/ukf/blob/master/#ref3).
* Optimised form of square-root Unscented Kalman filter for parameter
estimation, implemented as described in [[3]](https://github.com/sfwa/ukf/blob/master/#ref3).
This library makes use of the [Eigen](https://github.com/sfwa/ukf/blob/master/https://eigen.tuxfamily.org) 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](https://github.com/sfwa/ukf/blob/master/test/), 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:
```C++
enum MyFields {
LatLon,
Altitude,
Velocity,
Attitude
};
using MyStateVector = UKF::StateVector<
UKF::Field
>,
UKF::Field,
UKF::Field>,
UKF::Field
>;
```
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:
```C++
template <> constexpr real_t UKF::Parameters::AlphaSquared = 1e-6;
```
For a description of what the scaling parameters do, see [[2]](https://github.com/sfwa/ukf/blob/master/#ref2) or read
the comments in the [code](https://github.com/sfwa/ukf/blob/master/include/UKF/StateVector.h).
### Measurement vector
The measurement vector can be specialised in a similar way, but with the
choice of a fixed or dynamic measurement vector:
```C++
enum MyFields {
StaticPressure,
DynamicPressure,
Accelerometer,
Gyroscope
};
using MyMeasurementVector = UKF::FixedMeasurementVector<
UKF::Field>,
UKF::Field>,
UKF::Field,
UKF::Field
>;
```
or:
```C++
enum MyFields {
StaticPressure,
DynamicPressure,
Accelerometer,
Gyroscope
};
using MyMeasurementVector = UKF::DynamicMeasurementVector<
UKF::Field>,
UKF::Field>,
UKF::Field,
UKF::Field
>;
```
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:
```C++
using MyCore = UKF::Core<
MyStateVector,
MyMeasurementVector,
UKF::IntegratorRK4
>;
```
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](https://github.com/sfwa/ukf/blob/master/include/UKF/Integrator.h).
The square-root state estimation filter can be used instead like this:
```C++
using MyCore = UKF::SquareRootCore<
MyStateVector,
MyMeasurementVector,
UKF::IntegratorRK4
>;
```
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:
```C++
using ProcessModelTestStateVector = UKF::StateVector<
UKF::Field>,
UKF::Field>
>;
template <> template <>
ProcessModelTestStateVector ProcessModelTestStateVector::derivative<>() const {
ProcessModelTestStateVector temp;
/* Position derivative. */
temp.set_field(get_field());
/* Velocity derivative. */
temp.set_field(UKF::Vector<3>(0, 0, 0));
return temp;
}
```
Also, the process model can take an arbitrary number of user-specified
inputs, like this:
```C++
template <> template <>
ProcessModelTestStateVector ProcessModelTestStateVector::derivative>(
const UKF::Vector<3>& acceleration) const {
ProcessModelTestStateVector temp;
/* Position derivative. */
temp.set_field(get_field());
/* Velocity derivative. */
temp.set_field(acceleration);
return temp;
}
```
### 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:
```C++
using MyMeasurementVector = UKF::DynamicMeasurementVector<
UKF::Field,
UKF::Field,
UKF::Field>,
UKF::Field>
>;
using MyStateVector = UKF::StateVector<
UKF::Field>,
UKF::Field>,
UKF::Field,
UKF::Field
>;
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
(const MyStateVector& state) {
return state.get_field() * UKF::Vector<3>(0, 0, -9.8);
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
(const MyStateVector& state) {
return state.get_field();
}
template <> template <>
real_t MyMeasurementVector::expected_measurement
(const MyStateVector& state) {
return 101.3 - 1.2*(state.get_field() / 100.0);
}
template <> template <>
real_t MyMeasurementVector::expected_measurement
(const MyStateVector& state) {
return 0.5 * 1.225 * state.get_field().squaredNorm();
}
```
As with the process model, the measurement model can take an arbitrary number
of user-specified inputs:
```C++
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
>(const MyStateVector& state, const UKF::Vector<3>& input) {
return state.get_field() * UKF::Vector<3>(0, 0, -9.8) + input;
}
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
>(const MyStateVector& state, const UKF::Vector<3>& input) {
return state.get_field();
}
template <> template <>
real_t MyMeasurementVector::expected_measurement
>(const MyStateVector& state, const UKF::Vector<3>& input) {
return 101.3 - 1.2*(state.get_field() / 100.0);
}
template <> template <>
real_t MyMeasurementVector::expected_measurement
>(const MyStateVector& state, const UKF::Vector<3>& input) {
return 0.5 * 1.225 * state.get_field().squaredNorm();
}
```
### Initialisation
The filter state, covariance, process noise covariance and measurement noise
covariance should be initialised to appropriate values, e.g.:
```C++
MyCore test_filter;
test_filter.state.set_field(UKF::Vector<3>(0, 0, 0));
test_filter.state.set_field(UKF::Vector<3>(0, 0, 0));
test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0));
test_filter.state.set_field(UKF::Vector<3>(0, 0, 0));
test_filter.covariance = MyStateVector::CovarianceMatrix::Zero();
test_filter.covariance.diagonal() << 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10;
test_filter.process_noise_covariance = MyStateVector::CovarianceMatrix::Identity()*1e-5;
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:
```C++
MyCore test_filter;
test_filter.state.set_field(UKF::Vector<3>(0, 0, 0));
test_filter.state.set_field(UKF::Vector<3>(0, 0, 0));
test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0));
test_filter.state.set_field(UKF::Vector<3>(0, 0, 0));
test_filter.root_covariance = MyStateVector::CovarianceMatrix::Zero();
test_filter.root_covariance.diagonal() << 100, 100, 100, 10, 10, 10, 1, 1, 2.2, 3.2, 3.2, 3.2;
test_filter.process_noise_root_covariance = MyStateVector::CovarianceMatrix::Identity()*3e-2;
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;
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:
```C++
MyMeasurementVector meas;
meas.set_field(UKF::Vector<3>(0.0, 0.0, 9.8));
meas.set_field(UKF::Vector<3>(0.0, 0.0, 0.0));
meas.set_field(101300.0);
meas.set_field(101300.0);
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:
```C++
MyMeasurementVector meas;
meas.set_field(UKF::Vector<3>(0.0, 0.0, 9.8));
meas.set_field(UKF::Vector<3>(0.0, 0.0, 0.0));
meas.set_field(101300.0);
meas.set_field(101300.0);
test_filter.a_priori_step(0.01);
/*
At this point, the state and covariance (or root_covariance) variables
reflect the a priori state and (root_)covariance.
*/
test_filter.innovation_step(meas);
/* Innovation and innovation_(root_)covariance variables are now set. */
test_filter.a_posteriori_step();
/* 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:
git clone git@github.com:sfwa/ukf.git && cd ukf
Then, configure CMake:
cmake -B ./build -DCMAKE_BUILD_TYPE=Release
Then build:
cmake --build ./build --config Release
Build and run the unit test suite:
cd build && make unittest && test/unittest && cd ..
If desired, build and run the benchmark suite:
cd build && make benchmark && benchmark/benchmark && cd ..
## Examples
Some examples are provided [here](https://github.com/sfwa/ukf/blob/master/examples/).
## 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
近期下载者:
相关文件:
收藏者: