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.14-pp39-pypy39_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 966c3c026aefab9308b80fa165ecc32412f4419ff0f85db16d16d2489aff7fb8 |
|
MD5 | 6861d22f0be770d22985049e6a301c06 |
|
BLAKE2b-256 | 1e6ab19bf83fa025fb906794c54db66ad63c3724752a20a27a6ba36b7e1bbfd8 |
Hashes for cnkalman-0.1.14-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 81fdd8352352d924e664ac9e9187c3ae3c73459302453e2ac405bc1da39db868 |
|
MD5 | 7e7465f9115e53a207d3587a34b2ba93 |
|
BLAKE2b-256 | 92534e01240395738d13da18d69c8e4609309bd35984228d18a1a71bc8726e35 |
Hashes for cnkalman-0.1.14-pp38-pypy38_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 0fbf1b775d7e50f5a1decc8ad75fb43396acd207a1a5938056b0536c9e139b0b |
|
MD5 | 6f0ee99b7a62203429a2b3252ab190cf |
|
BLAKE2b-256 | 03a898ae7c594595d81c9097d37cdf2992403f0a49606d951b7273e7dadfa5e7 |
Hashes for cnkalman-0.1.14-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 8aabb2d3d732c737658abf87d12eb3e0580275005b86e3ea43f66d67f82283be |
|
MD5 | c20c2f9009118b846880b6b4b093566e |
|
BLAKE2b-256 | 7c7dfbc8e17b7f3f47080b3757b49bdf082bf073ba081b77a5d3027f48ceda27 |
Hashes for cnkalman-0.1.14-pp37-pypy37_pp73-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 41a2e421291fc847e3f41eac05cd206cfcac55ceccc3fabbabbee74b94b61c7c |
|
MD5 | 75f323b079ecf0d8aa9433d42279423b |
|
BLAKE2b-256 | 846df0affdab0d2a718925225716e42188c22c788cbf3f38d052b87bacf3c3c5 |
Hashes for cnkalman-0.1.14-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 3678e97b64ddd864fb067594ecd3b1f46c23231a76f4ea55b498442b78cc9695 |
|
MD5 | 3739481592a9bc69430f316d36b01192 |
|
BLAKE2b-256 | 99718ac9303c23ede216d0d4cf59d36e063ce639d29a138bbf35b9e8f1648419 |
Hashes for cnkalman-0.1.14-cp311-cp311-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 16518b5baff98b3ece16028edbea787c49b94ef9f7d88d6ce1f6248596688396 |
|
MD5 | eec4d2bdbf13f6fa8c3da81febc93405 |
|
BLAKE2b-256 | 7651a7b2b688206088658eb2758a23ea5275a9603c7312a6d50383c23250d09b |
Hashes for cnkalman-0.1.14-cp311-cp311-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | febd92d2f2fc9af05d82953575f716ed49e55aae6102db471cabbed4e0a21c6e |
|
MD5 | ec7cfef411b98cadc42a9a44e9add6f8 |
|
BLAKE2b-256 | dbd8a3039de29098b6edb6e2b824490bd17e69cd31aa2d398459b7d3bb83cc7b |
Hashes for cnkalman-0.1.14-cp311-cp311-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 2bf9a38d786a785c4be705079aa7dc62243cd484bfccc38b2ce4c042d798a30e |
|
MD5 | ab18dfc81e915fe770aeaed9f9fbb6b9 |
|
BLAKE2b-256 | 0f912325972beec8957c7e9759acbd62a03f64ae8e2ebd90de376001bf492933 |
Hashes for cnkalman-0.1.14-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 5f599e3d719e1cd5f2b0609f1f8266de7abb7b29e091b040b632ac544a48b5e3 |
|
MD5 | 769fdfa9a172fb4b52f20c1c71b11689 |
|
BLAKE2b-256 | 683cbd463435a159e254dc071c4c57776c34f4584693454c7c86070d19974e2e |
Hashes for cnkalman-0.1.14-cp310-cp310-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 9347e2d9812df5af4dde2e3085c1209a82e4880d79482903cc212231f6c293c0 |
|
MD5 | d4d43d6919b6d0cf3290c0289756bf9f |
|
BLAKE2b-256 | c0e6716cc99158dc009eee3c1e4abe232ecc498a6b163bcc3da35ece27e30233 |
Hashes for cnkalman-0.1.14-cp310-cp310-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 609072699c68ca729f0d119fdd954511cb987e21182ce7a9ec052434bbd429dd |
|
MD5 | 751620fc61acada1a6f04a44e7cae955 |
|
BLAKE2b-256 | c7a5b45403d5b984a15910373b65e2a0410f319d55161e06a7b98918da12910b |
Hashes for cnkalman-0.1.14-cp310-cp310-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 37eff6c3595c84162f1b278f2bcd2e67a7ee0dee7e2bbdc7b735f75cd97a1ddf |
|
MD5 | d3ae2aff18e00396fcabd0c860cee53a |
|
BLAKE2b-256 | 8be52fb4489d4d5d03358a8443ce8342b21181c77a64a7f54297e34ba9a13c28 |
Hashes for cnkalman-0.1.14-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | e5eb2e952bfcc9814a4816ce8ac8a64592d01e2d82b55ef7b7034b95a8c53106 |
|
MD5 | 84b4ea9b445602745e9ca1b881f4c03c |
|
BLAKE2b-256 | d98455a3934d216f7cfb913020307aa19175c72827f9063f8dcf070c18c83077 |
Hashes for cnkalman-0.1.14-cp39-cp39-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 59751699933b8aa1f541d5d28e2bda37be0da7f36a90dbb992ada467182fd9c8 |
|
MD5 | 97c80a47df36eb3f7ba3d72768a63085 |
|
BLAKE2b-256 | 6120853725ae9dbe42cd4c7df9ec8f78af43b4b9b4bdec8131494826e938d9f8 |
Hashes for cnkalman-0.1.14-cp39-cp39-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 75c240ce5d3aab3e104d482421aafc9eb70ae31f6bf1d7e851ff929ddd55a728 |
|
MD5 | aa1490e6626ec8e84d0b69e0f4b6ce65 |
|
BLAKE2b-256 | 813b172ffd1fe7d0bec1a2f14251c6d61a60ecaa9368faf5036e196976de6c86 |
Hashes for cnkalman-0.1.14-cp39-cp39-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 701e398c44b3ca07ac5e0910094eb3d9e5b78b9113cdda4d6934645ec8d47f24 |
|
MD5 | 235ff8b032039b92993d61ff00d1f0ec |
|
BLAKE2b-256 | 39daffe872245c22e955ba01dfba6a53a745e625133e85bbe321a8881bf9939d |
Hashes for cnkalman-0.1.14-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 978d4be9274c7e024b375ddeab12bd15cdaa239238676a3591ebe3fb81da8137 |
|
MD5 | e5a6e201abc882c9491497f69db74496 |
|
BLAKE2b-256 | 9775875cbd11feae91ef35504671f923f95217af3d848d4353f3a4b126464fa9 |
Hashes for cnkalman-0.1.14-cp38-cp38-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 9d26c4119b48624ee1aafdf27670c4a8d2a487360db7d46693544a03d3e1ebf1 |
|
MD5 | e3295adb2f06fa66335cadedbeb102bf |
|
BLAKE2b-256 | 57bdc1d35cd8655ea8463cfe76b790e01bef6150b2a8c72d53c5263d8b3bcee2 |
Hashes for cnkalman-0.1.14-cp38-cp38-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 4703fbb60ba313dfb0eee360458ae50c47d4489fed3742ea8b1d352d3cee1fe6 |
|
MD5 | 126f62410890e94dcddb869969303a28 |
|
BLAKE2b-256 | c9bbbfdbde263436cc9e21b1955ae601d495f4f990c63b7a8aabc4890ae8bacf |
Hashes for cnkalman-0.1.14-cp38-cp38-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | e66da132015e5495fad3b4b87a39619e40a2b0116500d832e0dbee3035d42092 |
|
MD5 | a730c866538328494a3b75159d592fdf |
|
BLAKE2b-256 | c575bc4697d7c17d0cf418b0beea5f8314618e0ecab56832fe5a994ff85d9f1e |
Hashes for cnkalman-0.1.14-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | ff16697b7bca88aae66340100bef984e0311e2944f15807e0e56600bea3e068a |
|
MD5 | c593cb6d2fa6eaf61cd0e0de17737325 |
|
BLAKE2b-256 | f3c24d6d501c15927052123ccda23c81ccf3981952b4a81a5ae70025ecc4ecc6 |
Hashes for cnkalman-0.1.14-cp37-cp37m-win_amd64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 4722130acc4a2f3f5d7eea905b2f662257b5090e97d8191f872bb5e4045f827a |
|
MD5 | 53d4f9e4722bb732c54d53a5ed8b569b |
|
BLAKE2b-256 | 405aa36cfbea23289e6f2958f8a8f3ed02fd74b17f787657e970fb4d5d10baeb |
Hashes for cnkalman-0.1.14-cp37-cp37m-musllinux_1_1_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | 3beb5f56edb8b46b6b966e1651034c4e4bf4fcf9f525cdb7f32f86b95c603a84 |
|
MD5 | dfe47d39680f9ef4bb47a5d1fb849a07 |
|
BLAKE2b-256 | d98674136cb42cd146c5a4034c1378b968db2c72bead6130572da54a9763534f |
Hashes for cnkalman-0.1.14-cp37-cp37m-musllinux_1_1_i686.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | cd1023cd3b676a639501dc65b466971ec184532bdc149437f6d30685353a8625 |
|
MD5 | a02bc80d95f6841572ec1c4089bb2900 |
|
BLAKE2b-256 | d083ad74a93a9192371f73cf85468fcf5f59bcce7d193c2b3ec805300698e992 |
Hashes for cnkalman-0.1.14-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | fc6013fb875acaf435486d8b98c83025f68d26fe0933f35cc4dfce3e459243a1 |
|
MD5 | a031310c83c2ec8835e8c3011155c8ea |
|
BLAKE2b-256 | 2eb18da242d3948152de43dd3afca178070681412cc0c6974b8bd4ae2351eaf9 |