Package: cnkalman
CNKalmanThis 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 TheoryThere 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 depthtutorial by Roger Labbe.FeaturesSupport forextended kalman filter,linear kalman filters, andIterate Extended Kalman Filter(paper)Support foradaptive measurement covarianceSupport forerror-state kalman filterBuilt-in support for numerical-based jacobians, and an option to debug user provided jacobians by using
the numerical resultsMinimal heap allocationsSupports multiple measurement models per filter, which can be integrated at varying frequenciesC++ bindings for objected oriented applicationsAutomatic code generation for analytical jacobiansQuick startkalman_example.c#include<cnkalman/kalman.h>#include<stdio.h>staticinlinevoidkalman_transition_model_fn(FLTdt,conststructcnkalman_state_s*k,conststructCnMat*x0,structCnMat*x1,structCnMat*F){// Logic to fill in the next state x1 and the associated transition matrix F}staticinlinevoidkalman_process_noise_fn(void*user,FLTdt,conststructCnMat*x,structCnMat*Q){// Logic to fill in the process covariance Q}staticinlineboolkalman_measurement_model_fn(void*user,conststructCnMat*Z,conststructCnMat*x_t,structCnMat*y,structCnMat*H_k){// Logic to fill in the residuals `y`, and the jacobian of the predicted measurement function `h`returnfalse;// This should return true if the jacobian and evaluation were valid.}intmain(){intstate_cnt=1;cnkalman_state_tkalman_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_tkalman_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;CnMatZ,R;// Logic to fill in measurement matrix Z and measurement covariance matrix Rcnkalman_meas_model_predict_update(1,&kalman_meas_model,0,&Z,&R);printf("Output:%f\n",cn_as_vector(&kalman_state.state)[0]);return0;}Code GenerationOne 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.BikeLandmarks.pyfromsymengineimportatan2,asin,cos,sin,tan,sqrtimportcnkalman.codegenascg@cg.generate_code(state=3,u=2)defpredict_function(dt,wheelbase,state,u):x,y,theta=statev,alpha=ud=v*dtR=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)defmeas_function(state,landmark):x,y,theta=statepx,py=landmarkhyp=(px-x)**2+(py-y)**2dist=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 likestateanduabove, you must specify a size hint.When ran, this python script generates the companionBikeLandmarks.gen.hwhich has the generated code, and callouts
such as:staticinlinevoidgen_predict_function(CnMat*out,constFLTdt,constFLTwheelbase,constFLT*state,constFLT*u);staticinlinevoidgen_predict_function_jac_state(CnMat*Hx,constFLTdt,constFLTwheelbase,constFLT*state,constFLT*u);If you include this project in as a cmake project, a cmake functioncnkalman_generate_codeis available that makes this
an optional part of your build process.