Discrete Linear Multivariate Kalman Filter for Kinematic Systems

Linear Kalman Filter

This function works as a Discrete Multivariate Kalman Filter for kinematic systems (system that can be modeled using Newton’s equations of motion) and have linear Measurement functions.
The filter has features like estimation of process noise Covariance and bad measurement data detection. So user need not know the noise Covariance matrix (Q). The filter estimates estimates and updates this matrix using the variance of previous 'N' number of state estimates (Posteriors).

Bad measurement data is identified based by calculated Residuals. If the absolute magnitude of the residual is greater than a threshold set in terms of standard deviation from predicted variance (prior variance), then the measurement data is replaced by the predicted state (Prior).

% Example: A dog moving in single direction with const. velocity and observable position
% direct measurable state: x (position)
% hidden state: x' (velocity)

% difference equation:
% x_k+1=x_k + x'_k*dt
% x'_k+1=x'_k +err (constant velocity assumed, err is the erroneous acceleration)
% state transition matrix (F)==>[1 dt; 0 1]
% [x_k+1; x'_k+1]= [1 dt; 0 1]*[x_k; x'_k];

%% Outputs and Inputs

% dim: Dimension of the problem: In above example as dimension 2 (No. of states : position and velocity)

% updateState: Updated posterior of states (k_k+1)

% updateStateVar: Updated process covariance matrix of the system (P_k+1)

% initStateBelieve: Initial assumed states eg. [1 1] for assumed position and velocity, [1 1 1] for pos, vel, and acceleration.

% initVarBelieve: Initial assumed variance of states eg. [100 0; 0 50] for variance of position and velocity

% Q is process variance and is mentioned if known beforehand
% Q=[] if unknown

% Calcuation of Q
% For this problem, the variance in the process comes form erroneous acceleration (as the process is a constant velocity one)
% Variance in states [x; x'] due to accelation e ==> [dt^2/2; dt]*velVar
% Process Noise Covariace Q = [dt^2/2; dt]*[dt^2/2 dt]*velVar = [0.25*dt^4 0.5*dt^3;0.5*dt^3 dt^2]*velVar
% where velVar is the variance of the velocity and dt is timestep eg. 1,0.5,0.1 (say measurement rate of the sensor)
% Refer book "Estimation with Applications To Tracking and Navigation" by bar shalom p.303

% z is measurements vector/matrix.
% H Measurement-State Relation Matrix

% R Measurement Error covariance Matrix

% H=[1 0]; %measurment function, 1 for position and 0 for velocity (we are not measureing the velocity)

% badMeasCheck: 'on' to detect bad measurements, 'off' otherwise

% time step is very important, consider different effects of timestep on the formula for Q. smaller time steps are better.