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.12-pp39-pypy39_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 3039ea4f955a8fba299c451c766aa677bc1c06ea172123ab39817be99cb01ab7 |
|
MD5 | e0f1bd0facfd27de447e69efe2fee1c8 |
|
BLAKE2b-256 | 81d8eb90f0bc19caa81bf7968d4e5c28633f56057dfd4115bcf9adde9452ca2f |
Hashes for cnkalman-0.1.12-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | af4c674971474f2525cf94f0efa1f6e74b1af2dbe4d0e24760962d36b7742564 |
|
MD5 | bc019e2291fd31bb4fcdb3eeeab876da |
|
BLAKE2b-256 | 7f603538117768285997e79ba7d1e423215a31fab2102f3d0d7ac32d37b6d098 |
Hashes for cnkalman-0.1.12-pp38-pypy38_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | d2c6380cad0051d8ecbf6a91d7ca535eac5d0bc9dcddc3f9fd339cdb05ced1bf |
|
MD5 | 446ea863b4ade1641ed7deffc35b8d9c |
|
BLAKE2b-256 | 48b2328d70d479354c15b1fd1cb7308cd16589e9c6003e8c48820ea952bb2f88 |
Hashes for cnkalman-0.1.12-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | c7044a1988dcfbf0995e3238e22b879249211f766aa488ec176ead81e5e10b1e |
|
MD5 | 0ea255be08d58bcacc6222b852f0b26e |
|
BLAKE2b-256 | cf2298b95f2c5f01db9f248d98dbddf841b2afe846ff45a6fd1aee3b21363946 |
Hashes for cnkalman-0.1.12-pp37-pypy37_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | fe6dbdfb8378f43582040d1f61e763da68e3c4ffadb9ceb148c7491deaa5065f |
|
MD5 | d0ea6553f5d7fdbcc024006c22f2447a |
|
BLAKE2b-256 | fb47df72945bf6dfa29ad9d1efb0d949c43de3588325f6d92528031f4bcd74a4 |
Hashes for cnkalman-0.1.12-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | f7421ed929e5895601860e598a5dc6738c4b811567c59557e2ac23e13fed0e60 |
|
MD5 | 8e479707a9be9db8138f26d85f597aa7 |
|
BLAKE2b-256 | 7d17ab11110e2914f87ee70923258f1f3bbef46ff417cbd3bd69ff047ad14648 |
Hashes for cnkalman-0.1.12-cp311-cp311-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 1a54c39ddedcd6705e9775e9e7c6b7c680e2b0f819f415f2b862f9276de5a043 |
|
MD5 | 688c39655c3ec83398579b8facd53b20 |
|
BLAKE2b-256 | 210aa5b0a498e8dd8d4ae1c1db5c11dc6d0e0fd082d7817d432c0c554cdc1a7d |
Hashes for cnkalman-0.1.12-cp311-cp311-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 2f67876064406b87a7ca19ca6be274ca47a06a4cf241e64d37bb424a0882fad8 |
|
MD5 | 12e8c2674cdb0f8d699c0eea1ba4f697 |
|
BLAKE2b-256 | 30177476f68fa33bca6670568f1ce8f25230afc33ca8c2739ec2a8eb299f0f09 |
Hashes for cnkalman-0.1.12-cp311-cp311-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | a89797c7612e4e2bce4846ad11d72778fd5a9746d967e2adfb7fd8e449743c76 |
|
MD5 | c0010d6dd697dcb9cd3515729869db36 |
|
BLAKE2b-256 | 36fd41f11f5abb93a97bdef5a9b2857f5827c010acbe8a854b4281f64f96ea72 |
Hashes for cnkalman-0.1.12-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 7cd0e5042a91f8ec59c6a31928ec0c7efda48780606d92bb3cd1af9bcb826efb |
|
MD5 | 305d1528a89673766e001e1ed0b7538a |
|
BLAKE2b-256 | be47b14ae2f420324e9b279917f13c859f582a16b3546fe77c1cfce52220a528 |
Hashes for cnkalman-0.1.12-cp310-cp310-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 2b85c31d523865cbcedd0863a7686e0b3e31682b7c9c31b9f27666cee2534bee |
|
MD5 | 9e7a82a9680100843e85771baa9dacc3 |
|
BLAKE2b-256 | a5c152efaa37c0ccf19aaacc0f585bb51339e800cd31379263cc14f9eb868142 |
Hashes for cnkalman-0.1.12-cp310-cp310-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | c0f66c598843d52eb1562048631b1e41537adcf1eff23ecd23563a38fc2e6980 |
|
MD5 | 972f7fe29864a55c9a8ff2825e68e056 |
|
BLAKE2b-256 | 801455cc604cb3592d8e2a5d7d2df0983a0088800d8b97135ad434bc1bc09b7f |
Hashes for cnkalman-0.1.12-cp310-cp310-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | d79c7cb1c63b4e0eeab4e1244876bbff570d6feb164428a526b7f7ab3f100583 |
|
MD5 | be726d6a8329fac9abb0be5a887648af |
|
BLAKE2b-256 | e9fc5a0dae644005f23618369774aa5d4b8a79e23a75491bfd6548bf97b39974 |
Hashes for cnkalman-0.1.12-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 68f1041a3bcd96fb5d877270ee9244ff68d19181757cf085c482711ead9caa6e |
|
MD5 | 01f889a9ec519fcfaa3a0fb9de91fc95 |
|
BLAKE2b-256 | 31d77159086ee5c8f246bc2d2f1877cd2adb40e6d815476bf8f658658210d846 |
Hashes for cnkalman-0.1.12-cp39-cp39-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | a7764814efd59ced6fa0942e4b101b6995e4a2faf26100722f87f0ec00c0258b |
|
MD5 | fdb1d2d4f6d6b1e29722b84c337a6d8c |
|
BLAKE2b-256 | 363754a68967e1e4ee5fabefd5111960d17fc6d7c0bb43225eda2166ab6ecd92 |
Hashes for cnkalman-0.1.12-cp39-cp39-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 00e4952b5b23172912ebfd927a54ee2c04d70938badf593588bb928cb4cd682d |
|
MD5 | 9576570c6b7dae7e0313a17cc682df6b |
|
BLAKE2b-256 | 9cfa088d2596e425b71e4c6f1800aa0773201c986eeb8d547e377a71dd8f9131 |
Hashes for cnkalman-0.1.12-cp39-cp39-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 4a889960ae3271401df431c563db048b8e93865b8f65188e376aa3180daf3c3a |
|
MD5 | 6e5857dee466a90cbc7cab2f2094f564 |
|
BLAKE2b-256 | b1b06de1e004dc8aba90f21ea13caab66f0f4ac524beed86c361b16051ca8f7a |
Hashes for cnkalman-0.1.12-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 92165195d1a523e35c0cd3bedf1a0ca3941f18de86e8d0671646430766912cef |
|
MD5 | b04b762fc297c643f2094a25f704eb01 |
|
BLAKE2b-256 | 8979c755ab42419743e9854f8d8d0669d8c7dc07009084af0a82c4e8b105a342 |
Hashes for cnkalman-0.1.12-cp38-cp38-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 6cf3852efbdeaf8cfe67d1137843acb59791028f354cc9a54f9c013869c28bf8 |
|
MD5 | 55cb5a67a0b9f12e8f1dce37ff424f03 |
|
BLAKE2b-256 | cc32331ff1a5e914cdb8fde1fc38265ec92aa42a1cfaf8c445a83a39f69ffbd2 |
Hashes for cnkalman-0.1.12-cp38-cp38-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | f21f61d39df68db03074c195f6fc6d92e5a316f53574a0ad172147e45d2eb6d6 |
|
MD5 | fbc8627d70d98fec5fc6d8a250154b23 |
|
BLAKE2b-256 | 1201a4872af30db1098aa4786dce0642616fb001b17f0f59e8c97e9234470c09 |
Hashes for cnkalman-0.1.12-cp38-cp38-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 0b967338f83d4f6712e4fbd026b36624a7b98ac25b1f3653ffe303f77e28f2cd |
|
MD5 | 4958227737db4a2bcb280779ea1d18fa |
|
BLAKE2b-256 | 4d3db9091912b5e832bd47bbb30b13027c05e57d9bc418af0b8ee79193e578ea |
Hashes for cnkalman-0.1.12-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 48dadaa944764da3f0616b351c328fbd5e2e9959f42801bc9982892b0845c3c4 |
|
MD5 | 9a7a16194d56ceab0787ef66d2dc15a3 |
|
BLAKE2b-256 | 9f51cb1d907190bd0e4e21d2708e13e36595f0b9097220199dba91c861e852de |
Hashes for cnkalman-0.1.12-cp37-cp37m-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | dccf88eec2d5262af5642382bacd7edb8b4389aca366c48c191ab22701696b5b |
|
MD5 | 9beae99abf1436974f093d98f81e8174 |
|
BLAKE2b-256 | dc2365aefba5aa3841af6b1ecc37c0d6b837840fe468ff9bbf3f6a152645c1a7 |
Hashes for cnkalman-0.1.12-cp37-cp37m-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 91cd7ae93e33829ea55c82a359a9514b1ac40d833512fb09a55d865286984a9f |
|
MD5 | 8c1dc8de3bbd2fbb66375c6f8a00016b |
|
BLAKE2b-256 | 2c8c9a2604e6ac207c8b5d606e138ec3945e32a9f009e55aa51ffcd2aef8fa54 |
Hashes for cnkalman-0.1.12-cp37-cp37m-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | e1ee0cc118dade1618c848fb6c1542af2fdba7432d1e0f6727b2f36a685db8ce |
|
MD5 | d1342b20e03b2ae8f0c4bea213124d12 |
|
BLAKE2b-256 | c031ec5d52c92c586d02802ae9b1f801c738c6eb29c60f05dd542e736b0c28ac |
Hashes for cnkalman-0.1.12-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 1d7a5d2e0a69fc965cd599e44bb1e823cd1f4bcf5b0c796c7909e570dfb5a3dd |
|
MD5 | 4612c9032b139b64f02706546382fae9 |
|
BLAKE2b-256 | 19bc79015e65193c81f1dbbe1eee982c1cc991d22115a1d47785349262b494fe |