How can you simulate algorithms using the Kalman filter?

The Kalman filter is a dynamic state observer based on a discrete state space model x(k+1) = Ax(k) + Bu(k) and y(k)=Cx(k) + Du(k), where x is a vector representing the internal states of an observed system, u is a vector of inputs and y is a vector of observed outputs (e.g. sensor readings) with white measurement noise. The Kalman filter is an optimal estimator wherein the mean value of the sum of the estimation errors (in the estimated state vector x) attains a minimal (in practice non-zero) value. The state space model is intrinsically linear, so it is not suitable for modelling non-linear dynamic systems. Parameters of the Kalman filter are estimated by solving the Ricatti equation.

The Kalman filter, a recursive algorithm for dynamic system state estimation, serves as a powerful algorithm simulator. By incorporating the system's dynamic model and measurements, it predicts the current state, updates with new measurements, and iteratively refines predictions. Users optimize accuracy by adjusting parameters and tuning covariance matrices. This iterative process, blending predictions with real-time measurements, enhances the Kalman filter's fidelity in tracking dynamic system states. e.g. for an autonomous vehicle navigating a city, estimate the position and velocity (based on sensor data). It can provide accurate representations amidst uncertainties, ensuring optimal state estimates in dynamic environments.

The Kalman Filter (KF) is the best linear minimum variance state estimator. What is a state? The state is the energy storage level of an energy storage element (position, orientation, velocity, angular velocity, temperature, pressure, concentration, electric/magnetic field/voltage/current). The KF estimates this state from available measurements, using a dynamic model of the system which is possible if the dynamics are observable from the measurements (look up observability and controllability in control systems). If the underlying dynamics are linear and the disturbances and measurement noise are normally distributed, the Kalman filter is the best state estimator in the minimum variance sense. It then is a maximum likelihood estimator.

The Kalman filter uses the process model (governing differential/difference equations), measurement noise, process noise and initial condition covariances (and cross covariances). Any algorithm updates that updates internal states iteratively can be considered the process model, besides various physical process models. The various uncertainties that enter into it--as with initial conditions, measurements, or round off errors, or human input disturbances/errors can be modeled as measurement noise and process noise with suitable variances. Once the uncertainties can be suitably described or approximated as normal distributions, it becomes possible to develop the Kalman filter that estimates/estimates/predicts the internal state of the process

I have done this work twice, for 2 experiments in High energy physics. C++ was a good frame within to write the algorithm. The Kalman filter is an ierative mathematical method that proceeds through 3 main steps: prediction, update, and smoothing. Tracking particles in detectors with this method is a successfully great idea, and it is proven since 2003 actually. The most tricky part for me was to paramerize the noise, and to make it propetly running with particles with very low monentum. In average, it showed to work!

A powerful means of using the Kalman filter is to incorporate the physical/geometric relationships between various sensed variables into the process model--distances, velocities and accelerations at different points on a rigid body aka vehicle for example. Similarly, invariance of known gradients of temperatures, chemical concentration, moisture levels or pressures or electric or magnetic fields are exploited.

One potential weakness of the Kalman filter for parameter estimation is that it is a statistical state estimation method, meaning that its state estimation will be optimal in a statistical sense, but not necessarily in a deterministic sense. This means that there is no guarantee that the state estimates will be correct in a deterministic (absolute) sense. Constraining the limits of the state estimates is tricky, and is the subject of much research.

The closer the underlying model to linear dynamics and the closer the underlying measurement and process noise are to being normally distributed, the closer the performance of a Kalman filter to a minimum variance estimator. When the deviations are large, the state estimate may not converge near actual values.

If you want to generate documented verifiable code, you may want to start from MATLAB/SIMULINK, particularly for aerospace or automotive applications. If the underlying uncertainties are not close to being normally distributed, using the Kalman filter is a bad idea. It is better to do the hard work of deriving the suitable maximum likelihood estimator with numerical methods. Examples are when noise is Rayleigh or Rician in cellular networks. Or we are trying to predict power grids or riverine floods which have power law probability densities of uncertainty. High quality coding is relatively easy once the problem is mapped to linear algebra and statistical calculations.