www.gusucode.com > ident_featured 案例代码 matlab源码程序 > ident_featured/KalmanFilterBlockExample.m

    %% State Estimation Using Time-Varying Kalman Filter
%
% This example shows how to estimate states of linear systems using
% time-varying Kalman filters in Simulink. You use the Kalman Filter block
% from the |System Identification Toolbox/Estimators| library to estimate
% the position and velocity of a ground vehicle based on noisy position
% measurements such as GPS sensor measurements. The plant model in Kalman
% filter has time-varying noise characteristics.

% Copyright 2014-2015 The MathWorks, Inc.

%% Introduction 
%
% You want to estimate the position and velocity of a ground
% vehicle in the north and east directions. The vehicle can move freely in
% the two-dimensional space without any constraints. You design a
% multi-purpose navigation and tracking system that can be used for any
% object and not just a vehicle.
%
% <<../ctrlKalmanNavigationExampleVehicleDiagram.png>>
%
% $x_e(t)$ and $x_n(t)$ are the vehicle's east and north positions from the
% origin, $\theta(t)$ is the vehicle orientation from east and $u_\psi(t)$
% is the steering angle of the vehicle. $t$ is the continuous-time
% variable.

%%
% The Simulink model consists of two main parts: Vehicle model and
% the Kalman filter. These are explained further in the following sections.
open_system('ctrlKalmanNavigationExample');

%% Vehicle Model
% The tracked vehicle is represented with a simple point-mass model:
%
% $$ \frac{d}{dt} \left[ 
% \begin{array} {c}
%  x_e(t) \\
%  x_n(t) \\
%  s(t) \\
%  \theta(t)
% \end{array} \right] = \left[ 
% \begin{array} {c}
%  s(t)\cos(\theta(t)) \\
%  s(t)\sin(\theta(t)) \\
%  (P \; \frac{u_T(t)}{s(t)} - A \; C_d \; s(t)^2) / m \\
%  s(t) \tan(u_\psi(t)) / L
% \end{array} \right]
% $$
%
% where the vehicle states are:
%
% $$ \begin{array} {ll}
% x_e(t) \; & \textnormal{East position} \; [m] \\
% x_n(t) \; & \textnormal{North position} \; [m] \\
% s(t) \; & \textnormal{Speed} \; [m/s] \\
% \theta(t) \; & \textnormal{Orientation from east} \; [deg] \\
% \end{array} $$
%
% the vehicle parameters are:
%
% $$ \begin{array} {ll}
% P=100000 \; & \textnormal{Peak engine power} \; [W] \\
% A=1 \; & \textnormal{Frontal area} \; [m^2] \\
% C_d=0.3 \; & \textnormal{Drag coefficient} \; [Unitless] \\ 
% m=1250 \; & \textnormal{Vehicle mass} \; [kg] \\
% L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\
% \end{array} $$
%
% and the control inputs are:
%
% $$ \begin{array} {ll}
% u_T(t) \; & \textnormal{Throttle position in the range of -1 and 1} \; [Unitless] \\
% u_\psi(t) \; & \textnormal{Steering angle} \; [deg] \\
% \end{array} $$
%
% The longitudinal dynamics of the model ignore tire rolling resistance.
% The lateral dynamics of the model assume that the desired steering angle
% can be achieved instantaneously and ignore the yaw moment of inertia.
%
% The car model is implemented in the |ctrlKalmanNavigationExample/Vehicle
% Model| subsystem. The Simulink model contains two PI controllers for
% tracking the desired orientation and speed for the car in the
% |ctrlKalmanNavigationExample/Speed And Orientation Tracking| subsystem.
% This allows you to specify various operating conditions for the car and
% test the Kalman filter performance.

%% Kalman Filter Design
% Kalman filter is an algorithm to estimate unknown variables of interest
% based on a linear model. This linear model describes the evolution of the
% estimated variables over time in response to model initial conditions as
% well as known and unknown model inputs. In this example, you estimate the
% following parameters/variables:
%
% $$ \hat{x}[n] = \left[
%   \begin{array}{c}
%      \hat{x}_e[n] \\
%      \hat{x}_n[n] \\
%      \hat{\dot{x}}_e[n] \\
%      \hat{\dot{x}}_n[n]
%   \end{array} 
%     \right]
% $$
%
% where
%
% $$ \begin{array} {ll}
% \hat{x}_e[n] \; & \textnormal{East position estimate} \; [m] \\
% \hat{x}_n[n] \; & \textnormal{North position estimate} \; [m] \\
% \hat{\dot{x}}_e[n] \; & \textnormal{East velocity estimate} \; [m/s] \\
% \hat{\dot{x}}_n[n] \; & \textnormal{North velocity estimate} \; [m/s] \\
% \end{array} $$
%
% The $\dot{x}$ terms denote velocities and not the derivative operator.
% $n$ is the discrete-time index. The model used in the Kalman filter is of
% the form:
%
% $$ \begin{array} {rl}
% \hat{x}[n+1] &= A\hat{x}[n] + Gw[n] \\
% y[n] &= C\hat{x}[n] + v[n]
% \end{array} $$
%
% where $\hat{x}$ is the state vector, $y$ is the measurements, $w$ is the
% process noise, and $v$ is the measurement noise. Kalman filter assumes
% that $w$ and $v$ are zero-mean, independent random variables with known
% variances $E[ww^T]=Q$, $E[vv^T]=R$, and $E[wv^T]=N$. Here, the A, G, and C
% matrices are:
%
% $$ A = \left[
%   \begin{array}{c c c c}
%      1 & 0 & T_s & 0 \\
%      0 & 1 & 0 & T_s \\
%      0 & 0 & 1 & 0 \\
%      0 & 0 & 0 & 1
%   \end{array} 
%     \right]
% $$
%
% $$ G = \left[
%   \begin{array}{c c}
%      T_s/2 & 0 \\
%      0 & T_s/2 \\
%      1 & 0 \\
%      0 & 1
%   \end{array} 
%     \right]
% $$
%
% $$ C = \left[
%   \begin{array}{c c c c}
%      1 & 0 & 0 & 0 \\
%      0 & 1 & 0 & 0
%   \end{array} 
%     \right]
% $$
%
% where $T_s=1\;[s]$

%%
% The third row of A and G model the east velocity as a random walk:
% $\hat{\dot{x}}_e[n+1]=\hat{\dot{x}}_e[n]+w_1[n]$. In reality, position is
% a continuous-time variable and is the integral of velocity over time
% $\frac{d}{dt}\hat{x}_e=\hat{\dot{x}}_e$. The first row of the A and G
% represent a disrete approximation to this kinematic relationship:
% $(\hat{x}_e[n+1]-\hat{x}_e[n])/Ts=(\hat{\dot{x}}_e[n+1]+\hat{\dot{x}}_e[n])/2$.
% The second and fourth rows of the A and G represent the same relationship
% between the north velocity and position.
%
% The C matrix represents that only position measurements are available. A
% position sensor, such as GPS, provides these measurements at the sample
% rate of 1Hz. The variance of the measurment noise $v$, the R matrix, is
% specified as $R=50$. Since R is specified as a scalar, the Kalman filter
% block assumes that the matrix R is diagonal, its diagonals are 50 and is
% of compatible dimensions with y. If the measurement noise is Gaussian,
% R=50 corresponds to 68% of the position measurements being within
% $\pm\sqrt{50}\;m$ or the actual position in the east and north
% directions. However, this assumption is not necessary for the Kalman
% filter.
%
% The elements of $w$ capture how much the vehicle velocity can change over
% one sample time Ts. The variance of the process noise w, the Q matrix, is
% chosen to be time-varying. It captures the intuition that typical values
% of $w[n]$ are smaller when velocity is large. For instance, going from 0
% to 10m/s is easier than going from 10 to 20m/s. Concretely, you use the
% estimated north and east velocities and a saturation function to
% construct Q[n]:
%
% $$f_{sat}(z)=min(max(z,25),625)$$
%
% $$ Q[n] = \left[
%   \begin{array}{ c c }
%       \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_e^2)} & 0 \\
%      0 & \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_n^2)}
%   \end{array} 
%     \right]
% $$
%
% The diagonals of Q model the variance of w inversely proportional to the
% square of the estimated velocities. The saturation function prevents Q
% from becoming too large or small. The coefficient 250 is obtained from a
% least squares fit to 0-5, 5-10, 10-15, 15-20, 20-25m/s acceleration time
% data for a generic vehicle. Note that the diagonal Q implies a naive
% approach that assumes that the velocity changes in north and east
% direction are uncorrelated.

%% Kalman Filter Block Inputs and Setup
% The 'Kalman Filter' block is in the |System Identification
% Toolbox/Estimators| library in Simulink.  It is also in |Control System
% Toolbox| library. Configure the block parameters for discrete-time state
% estimation. Specify the following *Filter Settings* parameters:
%
% * *Time domain:* Discrete-time. Choose this option to estimate
% discrete-time states.
%
% * Select the *Use current measurement y[n] to improve xhat[n]* check
% box. This implements the "current estimator" variant of the discrete-time
% Kalman filter. This option improves the estimation accuracy and is more
% useful for slow sample times. However, it increases the computational
% cost. In addition, this Kalman filter variant has direct feedthrough,
% which leads to an algebraic loop if the Kalman filter is used in a
% feedback loop that does not contain any delays (the feedback loop itself
% also has direct feedthrough). The algebraic loop can further impact the
% simulation speed.

%%
% Click the *Options* tab to set the block inport and outport options:
%
% * Unselect the *Add input port u* check box. There are no known inputs in
% the plant model.
%
% * Select the *Output state estimation error covariance Z* check box. The
% Z matrix provides information about the filter's confidence in the state
% estimates.
%
% <<../ctrlKalmanNavigationExampleScreenshot1.png>>

%%
% Click *Model Parameters* to specify the plant model and noise
% characteristics:
%
% * *Model source:* Individual A, B, C, D matrices.
%
% * *A*: A. The A matrix is defined earlier in this example.
%
% * *C*: C. The C matrix is defined earlier in this example.
%
% * *Initial Estimate Source*: Dialog
%
% * *Initial states x[0]*: |0|. This represents an initial guess of 0 for
% the position and velocity estimates at t=0s.
%
% * *State estimation error covariance P[0]*: |10|. Assume that the
% error between your initial guess x[0] and its actual value is a random
% variable with a standard deviation $\sqrt{10}$.
%
% * Select the *Use G and H matrices (default G=I and H=0)* check
% box to specify a non-default G matrix.
%
% * *G*: G. The G matrix is defined earlier in this example.
%
% * *H*: |0|. The process noise does not impact the measurments y entering
% the Kalman filter block.
%
% * Unselect the *Time-invariant Q* check box. The Q matrix is time-varying
% and is supplied through the block inport Q. The block uses a time-varying
% Kalman filter due to this setting. You can select this option to use a
% time-invariant Kalman filter. Time-invariant Kalman filter performs
% slightly worse for this problem, but is easier to design and has a lower
% computational cost.
%
% * *R*: R. This is the covariance of the measurement noise $v[n]$. The R
% matrix is defined earlier in this example.
%
% * *N*: |0|. Assume that there is no correlation between process and
% measurement noises.
%
% * *Sample time (-1 for inherited)*: Ts, which is defined earlier in this
% example.
%
% <<../ctrlKalmanNavigationExampleScreenshot2.png>>

%% Results
%
% Test the performance of the Kalman filter by simulating a scenario where the
% vehicle makes the following maneuvers:
%
% * At t = 0 the vehicle is at $x_e(0)=0$, $x_n(0)=0$ and is stationary.
%
% * Heading east, it accelerates to 25m/s. It decelerates to 5m/s at t=50s.
%
% * At t = 100s, it turns toward north and accelerates to 20m/s.
%
% * At t = 200s, it makes another turn toward west. It accelerates to
% 25m/s.
%
% * At t = 260s, it decelerates to 15m/s and makes a constant speed 180 degree
% turn.

%%
% Simulate the Simulink model. Plot the actual, measured and Kalman
% filter estimates of vehicle position.
sim('ctrlKalmanNavigationExample');

figure;
% Plot results and connect data points with a solid line.
plot(x(:,1),x(:,2),'bx',...
    y(:,1),y(:,2),'gd',... 
    xhat(:,1),xhat(:,2),'ro',...
    'LineStyle','-');
title('Position');
xlabel('East [m]');
ylabel('North [m]');
legend('Actual','Measured','Kalman filter estimate','Location','Best');
axis tight;
%%
% The error between the measured and actual position as well as the error
% between the kalman filter estimate and actual position is:

% East position measurement error [m]
n_xe = y(:,1)-x(:,1);
% North position measurement error [m]
n_xn = y(:,2)-x(:,2);
% Kalman filter east position error [m]
e_xe = xhat(:,1)-x(:,1);
% Kalman filter north position error [m]
e_xn = xhat(:,2)-x(:,2);

figure;
% East Position Errors
subplot(2,1,1);
plot(t,n_xe,'g',t,e_xe,'r');
ylabel('Position Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xe,1)/numel(n_xe)),sprintf('Kalman f.: %.3f',norm(e_xe,1)/numel(e_xe)));
axis tight;
% North Position Errors
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),'g',t,xhat(:,2)-x(:,2),'r');
ylabel('Position Error - North [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xn,1)/numel(n_xn)),sprintf('Kalman f: %.3f',norm(e_xn,1)/numel(e_xn)));
axis tight;
%%
% The plot legends show the position measurement and estimation error
% ($||x_e-\hat{x}_e||_1$ and $||x_n-\hat{x}_n||_1$) normalized by the
% number of data points. The Kalman filter estimates have about 25% percent
% less error than the raw measurements.

%%
% The actual velocity in the east direction and its Kalman filter estimate
% is shown below in the top plot. The bottom plot shows the estimation
% error.

e_ve = xhat(:,3)-x(:,3); % [m/s] Kalman filter east velocity error
e_vn = xhat(:,4)-x(:,4); % [m/s] Kalman filter north velocity error
figure;
% Velocity in east direction and its estimate
subplot(2,1,1);
plot(t,x(:,3),'b',t,xhat(:,3),'r');
ylabel('Velocity - East [m]');
xlabel('Time [s]');
legend('Actual','Kalman filter','Location','Best');
axis tight;
subplot(2,1,2);
% Estimation error
plot(t,e_ve,'r');
ylabel('Velocity Error - East [m]');   
xlabel('Time [s]');
legend(sprintf('Kalman filter: %.3f',norm(e_ve,1)/numel(e_ve)));
axis tight;
%%
% The legend on the error plot shows the east velocity estimation error
% $||\dot{x}_e-\hat{\dot{x}}_e||_1$ normalized by the number of data
% points.

%%
% The Kalman filter velocity estimates track the actual velocity trends
% correctly. The noise levels decrease when the vehicle is traveling at
% high velocities. This is in line with the design of the Q matrix. The
% large two spikes are at t=50s and t=200s. These are the times when the
% car goes through sudden decelearation and a sharp turn, respectively.
% The velocity changes at those instants are much larger than the
% predictions from the Kalman filter, which is based on its Q matrix input.
% After a few time-steps, the filter estimates catch up with the actual
% velocity.

%% Summary
%
% You estimated the position and velocity of a vehicle using the Kalman
% filter block in Simulink. The process noise dynamics of the model were
% time-varying. You validated the filter performance by simulating various
% vehicle maneuvers and randomly generated measurement noise. The Kalman
% filter improved the position measurements and provided velocity estimates
% for the vehicle.

%%
%
bdclose('ctrlKalmanNavigationExample');