Support tools for cnkalman
Project description
CNKalman ![Build and Test](https://pypi-camo.freetls.fastly.net/f18ce1e3583cec8f1c167943a37ad79a4f48a822/68747470733a2f2f6769746875622e636f6d2f636e746f6f6c732f636e6b616c6d616e2f616374696f6e732f776f726b666c6f77732f636d616b652e796d6c2f62616467652e737667)
This is a relatively low level implementation of a kalman filter; with support for extended and iterative extended kalman filters. The goals of the project are to provide a numerically stable, robust EKF implementation which is both fast and portable.
The main logic is written in C and only needs the associated matrix library to work; and there are C++ wrappers provided for convenience.
Tutorial on Kalman Filter Theory
There was originally going to be a more in depth discussion of the theoretical side but there isn't much one could do to improve on the very in depth tutorial by Roger Labbe.
Features
- Support for extended kalman filter, linear kalman filters, and Iterate Extended Kalman Filter (paper)
- Support for adaptive measurement covariance
- Support for error-state kalman filter
- Built-in support for numerical-based jacobians, and an option to debug user provided jacobians by using the numerical results
- Minimal heap allocations
- Supports multiple measurement models per filter, which can be integrated at varying frequencies
- C++ bindings for objected oriented applications
- Automatic code generation for analytical jacobians
Quick start
#include <cnkalman/kalman.h>
#include <stdio.h>
static inline void kalman_transition_model_fn(FLT dt, const struct cnkalman_state_s *k, const struct CnMat *x0,
struct CnMat *x1, struct CnMat *F) {
// Logic to fill in the next state x1 and the associated transition matrix F
}
static inline void kalman_process_noise_fn(void *user, FLT dt, const struct CnMat *x, struct CnMat *Q) {
// Logic to fill in the process covariance Q
}
static inline bool kalman_measurement_model_fn(void *user, const struct CnMat *Z, const struct CnMat *x_t,
struct CnMat *y, struct CnMat *H_k) {
// Logic to fill in the residuals `y`, and the jacobian of the predicted measurement function `h`
return false; // This should return true if the jacobian and evaluation were valid.
}
int main() {
int state_cnt = 1;
cnkalman_state_t kalman_state = { 0 };
cnkalman_state_init(&kalman_state, state_cnt, kalman_transition_model_fn, kalman_process_noise_fn, 0, 0);
// Uncomment the next line if you want to use numerical jacobians for the transition matrix
//kalman_state.transition_jacobian_mode = cnkalman_jacobian_mode_two_sided;
cnkalman_meas_model_t kalman_meas_model = { 0 };
cnkalman_meas_model_init(&kalman_state, "Example Measurement", &kalman_meas_model, kalman_measurement_model_fn);
// Uncomment the next line if you want to use numerical jacobians for this measurement
// kalman_meas_model.meas_jacobian_mode = cnkalman_jacobian_mode_two_sided;
CnMat Z, R;
// Logic to fill in measurement matrix Z and measurement covariance matrix R
cnkalman_meas_model_predict_update(1, &kalman_meas_model, 0, &Z, &R);
printf("Output:%f\n", cn_as_vector(&kalman_state.state)[0]);
return 0;
}
Code Generation
One of the more difficult things about extended kalman filters is the fact that calculating the jacobian for even a simple measurement or prediction function can be tedious and error prone. An optional portion of cnkalman is easy integration of symengine in such a way that you can write the objective function in python and it'll generate the C implementation of both the function itself as well as it's jacobian with each of it's inputs.
from symengine import atan2, asin, cos, sin, tan, sqrt
import cnkalman.codegen as cg
@cg.generate_code(state = 3, u = 2)
def predict_function(dt, wheelbase, state, u):
x, y, theta = state
v, alpha = u
d = v * dt
R = wheelbase/tan(alpha)
beta = d / wheelbase * tan(alpha)
return [x + -R * sin(theta) + R * sin(theta + beta),
y + R * cos(theta) - R * cos(theta + beta),
theta + beta]
@cg.generate_code(state = 3, landmark = 2)
def meas_function(state, landmark):
x, y, theta = state
px, py = landmark
hyp = (px-x)**2 + (py-y)**2
dist = sqrt(hyp)
return [dist, atan2(py - y, px - x) - theta]
There are limitations in what type of logic is permissible here -- it must be something that is analytically tractable -- but most objective functions themselves are not too complicated to write.
Notice that for array-type input like state
and u
above, you must specify a size hint.
When ran, this python script generates the companion BikeLandmarks.gen.h
which has the generated code, and callouts
such as:
static inline void gen_predict_function(CnMat* out, const FLT dt, const FLT wheelbase, const FLT* state, const FLT* u);
static inline void gen_predict_function_jac_state(CnMat* Hx, const FLT dt, const FLT wheelbase, const FLT* state, const FLT* u);
If you include this project in as a cmake project, a cmake function cnkalman_generate_code
is available that makes this
an optional part of your build process.
Project details
Release history Release notifications | RSS feed
Download files
Download the file for your platform. If you're not sure which to choose, learn more about installing packages.
Source Distributions
Built Distributions
Hashes for cnkalman-0.1.11-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | bdea7a17604a517d6ad1585b18787c29ba97b59109aa8b0117187b1a93aecb58 |
|
MD5 | ddb55f1b91c934f0196bc424a41ef036 |
|
BLAKE2b-256 | 3f48b370203b543276d19cd5282a0122d32f195ab8145d9109a9c59e1ecad801 |
Hashes for cnkalman-0.1.11-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 90142142d4d55f2cac16b999db8ce5fe2542823415c65bc5fc4ab5848c348ca1 |
|
MD5 | 3384f90c2cb80c64b616a62e3bfc95e3 |
|
BLAKE2b-256 | aed12e608054219c104c2328811bc54313c3850c44682da9008bf45020c6ef0f |
Hashes for cnkalman-0.1.11-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 464bafd7ab247df771ccaa187d4dd70752370d07c895ab14823b26cb79a7b024 |
|
MD5 | 248ca2054b0397d086f88474695a0459 |
|
BLAKE2b-256 | 31016069debdd0bedf086ddeca44c8a2d2b867cca3fce4dcdbe1c1a269354153 |
Hashes for cnkalman-0.1.11-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | c122d0825489212c77f9817e238a7968b31275b46fd33c2adce3f82e52b14236 |
|
MD5 | 03fd0a4ea184293b33a7addb939e83d4 |
|
BLAKE2b-256 | da616222f52f40ddf4afc7174cbb46ce41ed3ee24a2e6d81f1dbb1a47462b881 |
Hashes for cnkalman-0.1.11-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | d023a3b64b272326107b28328acecd7ff24c87a0ad9fab751c351be374a5dc17 |
|
MD5 | 77487f25814db74cdbf1aaa1563f3555 |
|
BLAKE2b-256 | 370db4801bc01c4f7a0123fb305bef307663f8f21fbd4288501e2d5a333a6b29 |
Hashes for cnkalman-0.1.11-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 499be69754aa95ebe3cc622d944d7f9aea4055e7417c6c4ac4757b7d67de71aa |
|
MD5 | 350b8e2bae6b7448cac48fa1ae77b50b |
|
BLAKE2b-256 | c735627a43df8608b56566cbb7a0a35207f949fa5fb8da4003fcb021e771133e |
Hashes for cnkalman-0.1.11-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 131d1f36ae47d47e1b05883d0519d4f182a97aef5747aa17b34b7afa0449c947 |
|
MD5 | 9366a151bbb6e96e9469d5fcc677f88d |
|
BLAKE2b-256 | a47678dbcd350fec7eb0cc188151fcf3dbadfb71e350e2a8388603cb241f20c0 |
Hashes for cnkalman-0.1.11-cp37-cp37m-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 07cce7cbd5bde1f6705235f84ac94b7e1af13dad8c236c60e7062f5d7752e49b |
|
MD5 | e06ec08b4d68237b1a48b48c9d1e6ddb |
|
BLAKE2b-256 | 7da234cff4050288e4397bf091c3adf07b6bf7cdcfcd269f2927554bff6e74c6 |
Hashes for cnkalman-0.1.11-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 1a10cb38b65b892cd171029be3d984e410bf537bd3a7e6173f5e16104e40461f |
|
MD5 | 78f074e081142db1475817f17a94c796 |
|
BLAKE2b-256 | 74106de6d8030151845557598980a7b5e015760cb29259bcc0b9e64c14b1e5d3 |