A Simple and Intuitive Introduction to Kalman Filter with MATLAB Examples

Kalman Filter For Beginners With Matlab Examples Pdf

If you are interested in learning about one of the most powerful and versatile tools for data analysis, estimation, and prediction, then you should definitely read this article. In this article, you will learn what a Kalman filter is, how it works, how to implement it in Matlab, and how to use it for various applications. You will also find some useful resources to help you learn more about Kalman filters and Matlab.

Kalman Filter For Beginners With Matlab Examples Pdf

What is a Kalman filter and why is it useful?

A Kalman filter is a mathematical technique that allows you to combine noisy and incomplete measurements from different sources to estimate the true state of a system or process. For example, you can use a Kalman filter to estimate the position and velocity of a car based on GPS, radar, and accelerometer data. A Kalman filter can also predict the future state of a system based on its current state and a model of its dynamics. For example, you can use a Kalman filter to predict the trajectory of a missile based on its initial launch conditions and a model of its motion.

A Kalman filter is useful because it can handle uncertainty and noise in the measurements and the model. It can also adapt to changing conditions and new information. A Kalman filter can provide optimal estimates and predictions in terms of minimizing the mean squared error. A Kalman filter can also be implemented efficiently using recursive equations that only require the previous state estimate and the current measurement.

The basic idea of a Kalman filter

The basic idea of a Kalman filter is to combine two sources of information: a prior estimate of the state based on the previous measurement and the model, and a measurement update based on the current measurement and the measurement model. The prior estimate represents what we know about the state before we see the current measurement. The measurement update represents how we correct the prior estimate based on the new information from the current measurement. The combination of these two sources of information results in a posterior estimate of the state that is more accurate than either source alone.

The prior estimate is obtained by applying the state transition model to the previous state estimate. The state transition model describes how the state evolves over time as a function of some inputs or controls. For example, the state transition model for a car moving at constant velocity can be written as: $$x_k = x_k-1 + v_k-1 \Delta t$$ where $x_k$ is the position at time step $k$, $v_k-1$ is the velocity at time step $k-1$, and $\Delta t$ is the time interval between two consecutive measurements.

The measurement update is obtained by applying the measurement model to the current measurement. The measurement model describes how the state relates to the measurement as a function of some parameters or coefficients. For example, the measurement model for a radar measuring the distance to a car can be written as: $$z_k = x_k + w_k$$ where $z_k$ is the measurement at time step $k$, $x_k$ is the position at time step $k$, and $w_k$ is the measurement noise at time step $k$.

The combination of the prior estimate and the measurement update is done by applying a weighted average formula that takes into account the uncertainty or variance of each source of information. The uncertainty or variance represents how much we trust or believe each source of information. The more uncertain or noisy a source of information is, the less weight it should have in the combination. The weighted average formula can be written as: $$\hatx_k = \hatx_k^- + K_k (z_k - \hatx_k^-)$$ where $\hatx_k$ is the posterior estimate of the state at time step $k$, $\hatx_k^-$ is the prior estimate of the state at time step $k$, $K_k$ is the Kalman gain at time step $k$, and $(z_k - \hatx_k^-)$ is the measurement residual or innovation at time step $k$. The Kalman gain is a factor that determines how much we trust the measurement update relative to the prior estimate. The Kalman gain can be computed as: $$K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R_k)^-1$$ where $P_k^-$ is the prior estimate of the state covariance matrix at time step $k$, $H_k$ is the measurement matrix that relates the state to the measurement, and $R_k$ is the measurement noise covariance matrix. The state covariance matrix represents how much uncertainty or variance there is in the state estimate. The measurement noise covariance matrix represents how much uncertainty or variance there is in the measurement.

The mathematical model of a Kalman filter

The mathematical model of a Kalman filter can be summarized by two sets of equations: the state equations and the measurement equations. The state equations describe how the state evolves over time as a function of some inputs or controls. The measurement equations describe how the state relates to the measurement as a function of some parameters or coefficients. The state equations and the measurement equations can be written in matrix form as: $$x_k = F_k x_k-1 + B_k u_k-1 + q_k-1$$ $$z_k = H_k x_k + w_k$$ where $x_k$ is the state vector at time step $k$, $F_k$ is the state transition matrix that relates the previous state to the current state, $B_k$ is the control input matrix that relates the control input to the state, $u_k-1$ is the control input vector at time step $k-1$, $q_k-1$ is the process noise vector at time step $k-1$, $z_k$ is the measurement vector at time step $k$, $H_k$ is the measurement matrix that relates the state to the measurement, and $w_k$ is the measurement noise vector at time step $k$. The process noise and the measurement noise are assumed to be zero-mean Gaussian random variables with known covariance matrices: $$q_k-1 \sim N(0, Q_k-1)$$ $$w_k \sim N(0, R_k)$$ where $Q_k-1$ is the process noise covariance matrix and $R_k$ is the measurement noise covariance matrix.

The steps of a Kalman filter algorithm

The steps of a Kalman filter algorithm can be summarized by two phases: prediction and update. The prediction phase uses the state equations to predict the prior estimate of the state and its covariance based on the previous state estimate and the control input. The update phase uses the measurement equations and the weighted average formula to update the posterior estimate of the state and its covariance based on the prior estimate and the current measurement. The prediction phase and the update phase can be written in pseudocode as: ``` # Prediction phase x_prior = F * x_posterior + B * u P_prior = F * P_posterior * F' + Q # Update phase y = z - H * x_prior S = H * P_prior * H' + R K = P_prior * H' * inv(S) x_posterior = x_prior + K * y P_posterior = (I - K * H) * P_prior ``` where `x_posterior` and `P_posterior` are the posterior estimate of the state and its covariance, `x_prior` and `P_prior` are the prior estimate of the state and its covariance, `z` is the current measurement, `y` is the measurement residual or innovation, `S` is the innovation covariance, `K` is How to implement a Kalman filter in Matlab?

Matlab is a popular software for numerical computing and data analysis. Matlab has many built-in functions and toolboxes that can help you implement and use Kalman filters for various applications. In this section, you will learn how to use some of the Matlab functions for Kalman filtering and see some examples of Kalman filtering in Matlab.

The Matlab functions for Kalman filtering

Matlab has several functions that can help you create, initialize, predict, update, and simulate Kalman filters. Some of the most useful functions are: - `kalman`: This function creates a Kalman filter object that contains the state equations and the measurement equations. You can specify the state transition matrix, the control input matrix, the measurement matrix, the process noise covariance matrix, and the measurement noise covariance matrix as input arguments. You can also specify other options such as the initial state estimate and its covariance. The function returns a Kalman filter object that you can use for prediction and update. - `predict`: This function performs the prediction phase of the Kalman filter algorithm. You can specify the Kalman filter object and the control input vector as input arguments. The function returns the prior estimate of the state and its covariance. - `correct`: This function performs the update phase of the Kalman filter algorithm. You can specify the Kalman filter object and the measurement vector as input arguments. The function returns the posterior estimate of the state and its covariance. - `kalmdemo`: This function launches a graphical user interface (GUI) that allows you to simulate and visualize Kalman filtering for different scenarios. You can choose from several predefined examples or create your own custom example. You can also change the parameters and options of the Kalman filter and see how they affect the performance and accuracy of the estimation and prediction.

A simple example of Kalman filtering in Matlab

To illustrate how to use the Matlab functions for Kalman filtering, let's consider a simple example of estimating the position and velocity of a car moving along a straight line with constant acceleration. The car has a GPS sensor that measures its position with some noise. The car also has an accelerometer that measures its acceleration with some noise. We want to use a Kalman filter to combine these two measurements and estimate the true position and velocity of the car.

The first step is to create a Kalman filter object using the `kalman` function. We need to specify the state equations and the measurement equations for this problem. The state vector consists of the position and the velocity of the car: $$x_k = \beginbmatrix p_k \\ v_k \endbmatrix$$ The state transition matrix is: $$F_k = \beginbmatrix 1 & \Delta t \\ 0 & 1 \endbmatrix$$ where $\Delta t$ is the time interval between two consecutive measurements. The control input vector consists of the acceleration of the car: $$u_k = \beginbmatrix a_k \endbmatrix$$ The control input matrix is: $$B_k = \beginbmatrix \frac12 \Delta t^2 \\ \Delta t \endbmatrix$$ The process noise vector is: $$q_k = \beginbmatrix q_p,k \\ q_v,k \endbmatrix$$ The process noise covariance matrix is: $$Q_k = \beginbmatrix \sigma_p,q^2 & 0 \\ 0 & \sigma_v,q^2 \endbmatrix$$ where $\sigma_p,q$ and $\sigma_v,q$ are the standard deviations of the process noise for the position and the velocity. The measurement vector consists of the GPS measurement of the position and the accelerometer measurement of the acceleration: $$z_k = \beginbmatrix z_p,k \\ z_a,k \endbmatrix$$ The measurement matrix is: $$H_k = \beginbmatrix 1 & 0 \\ 0 & 0 \endbmatrix$$ The measurement noise vector is: $$w_k = \beginbmatrix w_p,k \\ w_a,k \endbmatrix$$ The measurement noise covariance matrix is: $$R_k = \beginbmatrix \sigma_p,w^2 & 0 \\ 0 & \sigma_a,w^2 \endbmatrix$$ where $\sigma_p,w$ and $\sigma_a,w$ are the standard deviations of the measurement noise for the position and the acceleration. We can use the following Matlab code to create the Kalman filter object: ``` % Define the parameters and options of the Kalman filter dt = 0.1; % time interval sig_pq = 0.1; % standard deviation of position process noise sig_vq = 0.1; % standard deviation of velocity process noise sig_pw = 10; % standard deviation of position measurement noise sig_aw = 1; % standard deviation of acceleration measurement noise x0 = [0; 0]; % initial state estimate P0 = [100 0; 0 10]; % initial state covariance estimate % Create the Kalman filter object F = [1 dt; 0 1]; % state transition matrix B = [0.5*dt^2; dt]; % control input matrix H = [1 0; 0 0]; % measurement matrix Q = [sig_pq^2 0; 0 sig_vq^2]; % process noise covariance matrix R = [sig_pw^2 0; 0 sig_aw^2]; % measurement noise covariance matrix KF = kalman(F, B, H, Q, R, x0, P0); ``` The second step is to generate some simulated data for the car's position, velocity, acceleration, and measurements. We can use the following Matlab code to generate the data: ``` % Define the simulation parameters and variables T = 10; % simulation duration N = T/dt + 1; % number of time steps p_true = zeros(N,1); % true position v_true = zeros(N,1); % true velocity a_true = zeros(N,1); % true acceleration z_p = zeros(N,1); % GPS measurement of position z_a = zeros(N,1); % accelerometer measurement of acceleration % Generate the simulated data using random inputs and noises rng(1); % set random seed for reproducibility a_true(1) = randn * sig_aw; % initial acceleration for k = 2:N a_true(k) = a_true(k-1) + randn * sig_aw; % random walk acceleration model v_true(k) = v_true(k-1) + a_true(k-1) * dt + randn * sig_vq; % velocity with process noise p_true(k) = p_true(k-1) + v_true(k-1) * dt + 0.5 * a_true(k-1) * dt^2 + randn * sig_pq; % position with process noise z_p(k) = p_true(k) + randn * sig_pw; % GPS measurement with measurement noise z_a(k) = a_true(k) + randn * sig_aw; % accelerometer measurement with measurement noise end % Plot the simulated data figure; subplot(3,1,1); plot((0:N-1)*dt, p_true, 'b-', (0:N-1)*dt, z_p, 'r.'); xlabel('Time (s)'); ylabel('Position (m)'); legend('True position', 'GPS measurement'); subplot(3,1,2); plot((0:N-1)*dt, v_true, 'b-'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); legend('True velocity'); subplot(3,1,3); plot((0:N-1)*dt, a_true, 'b-', (0:N-1)*dt, z_a, 'r.'); xlabel('Time (s)'); ylabel('Acceleration (m/s^2)'); legend('True acceleration', 'Accelerometer measurement'); ``` The third step is to use the Kalman filter object and the simulated data to perform prediction and update at each time step. We can use the following Matlab code to perform the Kalman filtering: ``` % Define the estimation variables x_est = zeros(N,2); % state estimate vector P_est = zeros(N,2); % state covariance estimate vector % Perform Kalman filtering using prediction and update at each time step for k = 2:N u_k = z_a(k); % use accelerometer measurement as control input z_k = [z_p(k); z_a(k)]; % use GPS and accelerometer measurements as measurement vector [x_prior,P_prior] = predict(KF,u_k); % prediction phase [x_posterior,P_posterior] = correct(KF,z_k); % update phase P_est(k,:) = diag(P_posterior)'; % store state estimate and covariance end % Plot the estimation results figure; subplot(2,1,1); plot((0:N-1)*dt, p_true, 'b-', (0:N-1)*dt, z_p, 'r.', (0:N-1)*dt, x_est(:,1), 'g-'); xlabel('Time (s)'); ylabel('Position (m)'); legend('True position', 'GPS measurement', 'Kalman estimate'); subplot(2,1,2); plot((0:N-1)*dt, v_true, 'b-', (0:N-1)*dt, x_est(:,2), 'g-'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); legend('True velocity', 'Kalman estimate'); ``` A more complex example of Kalman filtering in Matlab

To illustrate how to use the Matlab functions for Kalman filtering for a more complex problem, let's consider an example of tracking and localizing a robot moving in a 2D plane. The robot has a camera that can detect landmarks in its environment and measure their relative angles and distances with some noise. The robot also has an odometer that can measure its relative displacement and orientation with some noise. We want to use a Kalman filter to combine these two measurements and estimate the true position and orientation of the robot.

The first step is to create a Kalman filter object using the `kalman` function. We need to specify the state equations and the measurement equations for this problem. The state vector consists of the position and orientation of the robot: $$x_k = \beginbmatrix x_k \\ y_k \\ \theta_k \endbmatrix$$ The state transition matrix is: $$F_k = \beginbmatrix 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \endbmatrix$$ The control input vector consists of the relative displacement and orientation measured by the odometer: $$u_k = \beginbmatrix d_k \\ \alpha_k \endbmatrix$$ The control input matrix is: $$B_k = \beginbmatrix \cos(\theta_k-1) & 0 \\ \sin(\theta_k-1) & 0 \\ 0 & 1 \endbmatrix$$ The process noise vector is: $$q_k = \beginbmatrix q_x,k \\ q_y,k \\ q_\theta,k \endbmatrix$$ The process noise covariance matrix is: $$Q_k = \beginbmatrix \sigma_x,q^2 & 0 & 0 \\ 0 & \sigma_y,q^2 & 0 \\ 0 & 0 & \sigma_\theta,q^2 \endbmatrix$$ where $\sigma_x,q$, $\sigma_y,q$, and $\sigma_\theta,q$ are the standard deviations of the process noise for the position and the orientation. The measurement vector consists of the relative angles and distances to the landmarks measured by the camera: $$z_k = \beginbmatrix z_\phi_1,k \\ z_r_1,k \\ z_\phi_2,k \\ z_r_2,k \\ ... \\ z_\phi_m,k \\ z_r_m,k \endbmatrix$$ where $m$ is the number of landmarks detected by the camera. The measurement matrix is: $$H_k = \beginbmatrix -\fracy_k - y_1(x_k - x_1)^2 + (y_k - y_1)^2 & \fracx_k - x_1(x_k - x_1)^2 + (y_k - y_1)^2 & -1 \\ \fracx_k - x_1\sqrt(x_k - x_1)^2 + (y_k - y_1)^2 & \fracy_k - y_1\sqrt(x_k - x_1)^2 + (y_k - y_1)^2 & 0 \\ -\fracy_k - y_2(x_k - x_2)^2 + (y_k - y_2)^2 & \fracx_k - x_2(x_k - x_2)^2 + (y_k - y_2)^2 & -1 \\ \fracx_k - x_2\sqrt(x_k - x_2)^2 + (y_k - y_2)^2 & \fracy_k - y_2\sqrt(x_k - x_2)^2 + (y_k - y_2)^2 & 0 \\ ... & ... & ... \\ -\fracy_k - y_m(x_k - x_m)^2 + (y_k - y_m)^2 & \fracx_k - x_m(x_k - x_m)^2 + (y_k - y_m)^2 & -1 \\ \fracx_k - x_m\sqrt(x_k - x_m)^2 + (y_k - y_m)^2 & \fracy_k - y_m\sqrt(x_k - x_m)^2 + (y_k - y_m)^2 & 0 \\ \endbmatrix$$ where $(x_i, y_i)$ are the coordinates of the landmarks in the global frame. The measurement noise vector is: $$w_k = \beginbmatrix w_\phi_1,k \\ w_r_1,k \\ w_\phi_2,k \\ w_r_2,k \\ ... \\ w_\phi_m,k \\ w_r_m,k \endbmatrix$$ The measurement noise covariance matrix is: $$R_k = \beginbmatrix \sigma_\phi,w^2 & 0 & 0 & 0 & ... & 0 & 0 \\ 0 & \sigma_r,w^2 & 0 & 0 & ... & 0 & 0 \\ 0 & 0 & \sigma_\phi,w^2 & 0 & ... & 0 & 0 \\ 0 & 0 & 0 & \sigma_r,w^2 & ... & 0 & 0 \\ ... & ... & ... & ... & ... & ... & ... \\ ... & 0 & \sigma_\phi,w^2 & 0 \\ 0 & 0 & 0 & 0 & ... & 0 & \sigma_r,w^2 \\ \endbmatrix$$ where $\sigma_\phi,w$ and $\sigma_r,w$ are the standard deviations of the measurement noise for the angles and the distances. We can use the following Matlab code to create the Kalman filter object: ``` % Define the parameters and options of the Kalman filter dt = 0.1; % time interval sig_xq = 0.1; % standard deviation of x position process noise sig_yq = 0.1; % standard deviation of y position process noise sig_thetaq = 0.1; % standard deviation of orientation process noise sig_phiw = 0.1; % standard deviation of angle measurement noise sig_rw = 1; % standard deviation of distance measurement noise x0 = [0; 0; 0]; % initial state estimate P0 = [100 0 0; 0 100 0; 0 0 10]; % initial state covariance estimate % Create the Kalman filter object F = eye(3); % state transition matrix B = zeros(3,2); % control input matrix H = zeros(2*m,3); % measurement matrix Q = diag([sig_xq^2 sig_yq^2 sig_thetaq^2]); % process noise covariance matrix R = diag(repmat([sig_phiw^2 sig_rw^2],1,m)); % measurement noise covariance matrix KF = kalman(F, B, H, Q, R, x0, P0); ``` The second step is to generate some simulated data for the robot's position, orientation, displacement, landmarks, and measurements. We can use the following Matlab code to generate the data: ``` % Define the simulation parameters and variables T = 10; % simulation duration N = T/dt + 1; % number of time steps x_true = zeros(N,3); % true position and orientation d_true = zeros(N,2); % true displacement and orientation change z_phi = zeros(N,m); % camera measurement of angles to landmarks z_r = zeros(N,m); % camera measurement of distances to landmarks % Generate the simulated data using random inputs and noises rng(1); % set random seed for reproducibility % Generate some rand