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.13-pp39-pypy39_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 4800e84aaa320065f336aafbe04f8260985aa02d93bd103a01b333e02875a7a1 |
|
MD5 | a35b2aa98fb8584cb36c81668489fbb2 |
|
BLAKE2b-256 | c50acbe6aef771725b73f87f844c6fdfe3636fbbe506a8ccdccd1c15d14e08bd |
Hashes for cnkalman-0.1.13-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 155d6329a2e2342b72e78b731bf61dc3533d59b0513288cb126eb0e2cc58e6ab |
|
MD5 | 5f7de7980fa19f60d8c2e39ef4e10933 |
|
BLAKE2b-256 | 947dab592b7ce0ed7e3946f45184c881aa87b6fab131a6c0d418a261057a555c |
Hashes for cnkalman-0.1.13-pp38-pypy38_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | abf1e80d564a138c69653a63e278084499b3e96ce14127d7c6a92b0674fedcbf |
|
MD5 | 66784c7df74f1e337b2ca1c831b8cc0a |
|
BLAKE2b-256 | 4e33a9ca73ee6acb321dedb990a7dacf4adb480dbf14cfe04f967bc34a04e7f2 |
Hashes for cnkalman-0.1.13-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | dd337282a489c72cc80ee06c060ba97db6ea1915d3e588deeb527a8cae48b792 |
|
MD5 | 02fdec5f19d7f2fcb74fdf1c8a49019a |
|
BLAKE2b-256 | ce8c36a020fea2c6cc9ef7ae5c417adcaf452b5b9fae9719f8cde667ab2781b3 |
Hashes for cnkalman-0.1.13-pp37-pypy37_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 5443cf35c2e8c239ddb327dd2b4843010cf1b80f815de0f8012761f861ac2255 |
|
MD5 | 46c63499f1b54ce5b285cd0d528fadb4 |
|
BLAKE2b-256 | 3f081678c165a95e1fda4b4e7d7ab907acd1d00d97297db3cc1c64a32cc399a6 |
Hashes for cnkalman-0.1.13-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 252fe72b88ec15a004fdbe8559d39f8f5732194ed0cc8dfe8d2021226feee876 |
|
MD5 | f591c0c6b5566f2e976bbfff50e583f5 |
|
BLAKE2b-256 | 6b46036be18ceb9e75ed21ec16659078cfc23aae4877ad0199d441376c2c09b9 |
Hashes for cnkalman-0.1.13-cp311-cp311-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | a2ec221158f5a3d1d55113058f15c435236b3508979d836f87f7509c11d83b8a |
|
MD5 | 193748d37020e1089feedfce82a3bced |
|
BLAKE2b-256 | 7bbffcb325dd41ade0aa61ff852d3bb98fbd3011cb6b7428e7adb1c95c52519f |
Hashes for cnkalman-0.1.13-cp311-cp311-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 0a0934b348e1d514af8619a6108d1d94d12f3450804d104b6810a021dfc69c4b |
|
MD5 | 9c582043bfe77fdeb5dbc52d45c7888e |
|
BLAKE2b-256 | 009d6e20b0d76117027b6a618b68074b670dcb2fed62adc8f0e8e5dd1c9a4694 |
Hashes for cnkalman-0.1.13-cp311-cp311-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 3e15554ca775d4b4ca8f62a6ff5b74f23cd3af59b740f7bbc4618bdda101da8f |
|
MD5 | 190980d3af788d31e0d4786e46c9f1fa |
|
BLAKE2b-256 | c1be08ae17696dcc5c0c531b6fce40a8a78a90e61db6e15c409bf656bb0698f1 |
Hashes for cnkalman-0.1.13-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 92e639b6d23b247bd79a2e745db27975a716d75147d037085e919fdd413f2580 |
|
MD5 | 29fb39867fff49225c2562dfcc51a40b |
|
BLAKE2b-256 | efd0a42410e1ef689a9c8c837c8f2687dd180b4b0b81d0d7ac391aa77a543c7e |
Hashes for cnkalman-0.1.13-cp310-cp310-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | af94bc35dee669f88e73d0e69c7b44206e07ce02dc2fbed96521e44de510fbe7 |
|
MD5 | a67b886d8c569ac081767d1d92b13833 |
|
BLAKE2b-256 | 290c2a5c9980eb68cfd576d7ca3063727c0a1c46cd7aebc1042def1cd9a53d6f |
Hashes for cnkalman-0.1.13-cp310-cp310-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | a6ca54da9b932fa71211848b655bd3f5bc11460cd34f148e8ff7e4863fe75314 |
|
MD5 | 7550dfcee4811abf9b1785c5f1fad9e1 |
|
BLAKE2b-256 | d6b8e12ec89b7073f5d99eda776f2183b8398910eae4b151404630d09e685d70 |
Hashes for cnkalman-0.1.13-cp310-cp310-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | a6e5e759e153d1d492f70eae5c85efaa119c221a8b24e86e764416582f0eea5b |
|
MD5 | da95d7d190c5d40e6a44738e4b498aa1 |
|
BLAKE2b-256 | 9f322f294509c17e34b8bcb311839ad8a8e252d223ab96fb34d4937f28a878e6 |
Hashes for cnkalman-0.1.13-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 00add33afb8cf509dd71347b9edd17f24f7bbb1bd15d026334fc64f59ce3e78e |
|
MD5 | 7f8f2b051778944502ad6b23c5834f47 |
|
BLAKE2b-256 | cf6cca8a002a0687d228c5eb03e17238589d4356493b81292e3c5beb3a8f3d1f |
Hashes for cnkalman-0.1.13-cp39-cp39-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 232c8d4e56c14aff470cb42e1228d70d5cfe388d7925ae57ec5897148d16573e |
|
MD5 | 839eef43126d2846aa054b0b378be7fc |
|
BLAKE2b-256 | 9d9dcbb16150a61fe4dc28aa0c7e0afacefaf02c16a82b2e742d972c47ddced6 |
Hashes for cnkalman-0.1.13-cp39-cp39-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | a3b5df0ceef3ba8ec1e13f5295b569a342961eca450a99fb504c2e115f452215 |
|
MD5 | 25e909676b36f2706222329a616d1a2f |
|
BLAKE2b-256 | ee9972c74669e2dc52e03ef6132fb9f533b9cd1c976b49f90b59ad99490858f0 |
Hashes for cnkalman-0.1.13-cp39-cp39-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | da84d7bcd1a75bdc9e3cec973fe4a6891b5aa82c6bc9d4728cfd4d5a821a003b |
|
MD5 | c3d3e357f65330a489a833222647d3dc |
|
BLAKE2b-256 | cbd04c9d91cc0204af95a66a94f188e14e8dd026ed08d708c37db2ca14fd3975 |
Hashes for cnkalman-0.1.13-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 06673a7ad68fe484cb034a06fb2a5c51d2aec4242a65634bdbfdcedaccf39fbb |
|
MD5 | 79a8d73878801cd356c45cddc9635e7f |
|
BLAKE2b-256 | 52c8d8c6e8737ec06a813fd6b10272b24ff9b390509abc0b0fcae1133e57c89f |
Hashes for cnkalman-0.1.13-cp38-cp38-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 8d339878a2e64a9fb6c7563dc34cc7062f4054e87e697c511626ac7021f324ce |
|
MD5 | 8ed1015716c8d6cf2457b11c00dc36ec |
|
BLAKE2b-256 | 25e13441cb33d98ce4476d147adfb89f6a962c737a45e408441091159b83ddd0 |
Hashes for cnkalman-0.1.13-cp38-cp38-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | f708e152b943364b8073db6404bcd35b0862ea30eb702a32e0746b60509eb627 |
|
MD5 | bdf073d3433b19024bdcfee84f54fbc4 |
|
BLAKE2b-256 | 3fb7338deaf0afcf6ef605ff911558df45be8058d276744ceda16f108e43d360 |
Hashes for cnkalman-0.1.13-cp38-cp38-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 96f8cfcc21ea1ac259712ab6ed8f982cb48b0f3131490dccdfa344f13850aa4f |
|
MD5 | 5b49656c9e053750f7ffaf3b4886b587 |
|
BLAKE2b-256 | 78abae3e10b1a4b1b7fdc0933ba42d6720bd74778a011a7aa789dfd29b31b930 |
Hashes for cnkalman-0.1.13-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | bb1f02e57a658ebf8575955622a46f430a0a4889f12693847e1c6f41f86e6588 |
|
MD5 | b5920419099a0bc35d765b8ea1847365 |
|
BLAKE2b-256 | 98b1dc4201602877a4e0a015ea238c5a1ca96f77386de67f2d7b5e7956a7132f |
Hashes for cnkalman-0.1.13-cp37-cp37m-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | a05698c16ddc8fddae88bbebfb9708b25fdc338fc3802eb94164322ba510257b |
|
MD5 | 73bd0dc0c6c9a4176a4e9b57b6c7f8ee |
|
BLAKE2b-256 | faeb67ed92226ad4e5332c6f0c7bb173a975690a189757e211e84f37eab4311a |
Hashes for cnkalman-0.1.13-cp37-cp37m-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | d2f00d177628e81e89560a471527f20be3c941f76d05821c843f0215c235f628 |
|
MD5 | ff72118b5e7f2ddba968bba9844b4df4 |
|
BLAKE2b-256 | ec89cb9a44bd1a22109074bc1d31b7f023dd2bb72d52fe2eea651220ed5b8c5a |
Hashes for cnkalman-0.1.13-cp37-cp37m-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 8bcc3183d97fb38305c46cb23a25b006dfd42a452d904970262c620225dd5bcd |
|
MD5 | 2e6cb7a700ce9c3e3ae4cdb0bbc7638a |
|
BLAKE2b-256 | ae0b317dbd08ebb7fe3d7f0f60eac5a0bbe30d887be57fffe095f1e75f03b672 |
Hashes for cnkalman-0.1.13-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 4d20a517faad3e8fb5e78c1098c7887edb0f0894ec62fe7d7e66c8338b551f9f |
|
MD5 | 856f699498cad5768bce19f808608298 |
|
BLAKE2b-256 | b0f0728a6cf93fb9379ad08161b1ca4db29d0aa16ddbc52a1ba26fac345307d5 |