estimate states of nonlinear system with multiple, multirate sensors -凯发k8网页登录

introduction

the toolbox has three simulink blocks for nonlinear state estimation:

these blocks support state estimation using multiple sensors operating at different sample rates. a typical workflow for using these blocks is as follows:

this example uses the extended kalman filter block to demonstrate the first two steps of this workflow. the last two steps are briefly discussed in the next steps section. the goal in this example is to estimate the states of an object using noisy measurements provided by a radar and a gps sensor. the states of the object are its position and velocity, which are denoted as xtrue in the simulink model.

if you are interested in the particle filter block, please see the example "parameter and state estimation in simulink using particle filter block".

plant modeling

the extended kalman filter (ekf) algorithm requires a state transition function that describes the evolution of states from one time step to the next. the block supports the following two function forms:

here f(..) is the state transition function, x is the state, and w is the process noise. u is optional, and represents additional inputs to f, for instance system inputs or parameters. additive noise means that the next state and process noise are related linearly. if the relationship is nonlinear, use the nonadditive form.

the function f(...) can be a matlab function that comply with the restrictions of matlab coder™, or a simulink function block. after you create f(...), you specify the function name and whether the process noise is additive or nonadditive in the extended kalman filter block.

in this example, you are tracking the north and east positions and velocities of an object on a 2-dimensional plane. the estimated quantities are:

here is the discrete-time index. the state transition equation used is of the nonadditive form , where is the state vector, and is the process noise. the filter assumes that is a zero-mean, independent random variable with known variance . the a and g matrices are:

where is the sample time. the third row of a and g model the east velocity as a random walk: . in reality, position is a continuous-time variable and is the integral of velocity over time . the first row of a and g represent a discrete approximation to this kinematic relationship: . the second and fourth rows of a and g represent the same relationship between the north velocity and position. this state transition model is linear, but the radar measurement model is nonlinear. this nonlinearity necessitates the use of a nonlinear state estimator such as the extended kalman filter.

in this example you implement the state transition function using a simulink function block. to do so,

analytical jacobian of the state transition function is also implemented in this example. specifying the jacobian is optional. however, this reduces the computational burden, and in most cases increases the state estimation accuracy. implement the jacobian function as a simulink function because the state transition function is a simulink function.

open_system('multirateekfexample')
open_system('multirateekfexample/simulink function - state transition jacobian');

sensor modeling - radar

the extended kalman filter block also needs a measurement function that describes how the states are related to measurements. the following two function forms are supported:

here h(..) is the measurement function, and v is the measurement noise. u is optional, and represents additional inputs to h, for instance system inputs or parameters. these inputs can differ from the inputs in the state transition function.

in this example a radar located at the origin measures the range and angle of the object at 20 hz. assume that both of the measurements have about 5% noise. this can be modeled by the following measurement equation:

here and are the measurement noise terms, each with variance 0.05^2. that is, most of the measurements have errors less than 5%. the measurement noise is nonadditive because and are not simply added to the measurements, but instead they depend on the states x. in this example, the radar measurement equation is implemented using a simulink function block.

open_system('multirateekfexample/simulink function - radar measurements');

sensor modeling - gps

a gps measures the east and north positions of the object at 1 hz. hence, the measurement equation for the gps sensor is:

here and are measurement noise terms with the covariance matrix [10^2 0; 0 10^2]. that is, the measurements are accurate up to approximately 10 meters, and the errors are uncorrelated. the measurement noise is additive because the noise terms affect the measurements linearly.

create this function, and save it in a file named gpsmeasurementfcn.m. when the measurement noise is additive, you must not specify the noise terms in the function. you provide this function name and measurement noise covariance in the extended kalman filter block.

type gpsmeasurementfcn

function y = gpsmeasurementfcn(x)
% gpsmeasurementfcn gps measurement function for state estimation
%
% assume the states x are:
%   [eastposition; northposition; eastvelocity; northvelocity]
%#codegen
% the %#codegen tag above is needed is you would like to use matlab coder to 
% generate c or c   code for your filter
y = x([1 2]); % position states are measured
end

filter construction

configure the extended kalman filter block to perform the estimation. you specify the state transition and measurement function names, initial state and state error covariance, and process and measurement noise characteristics.

in the system model tab of the block dialog, specify the following parameters:

state transition

initialization

since there are two sensors, click add measurement to specify a second measurement function.

measurement 1

measurement 2

in the multirate tab, since the two sensors are operating at different sample rates, perform the following configuration:

simulation and results

test the performance of the extended kalman filter by simulating a scenario where the object travels in a square pattern with the following maneuvers:

generate the true state values corresponding to this motion:

ts = 0.05; % [s] sample rate for the true states
[t, xtrue] = generatetruestates(ts); % generate position and velocity profile over 0-185 seconds

simulate the model. for instance, look at the actual and estimated velocities in the east direction:

sim('multirateekfexample');
open_system('multirateekfexample/scope - east velocity');

the plot shows the true velocity in the east direction, and its extended kalman filter estimates. the filter successfully tracks the changes in velocity. the multirate nature of the filter is most apparent in the time range t = 20 to 30 seconds. the filter makes large corrections every second (gps sample rate), while the corrections due to radar measurements are visible every 0.05 seconds.

next steps

summary

this example has shown how to use the extended kalman filter block in system identification toolbox. you estimated position and velocity of an object from two different sensors operating at different sampling rates.

close_system('multirateekfexample', 0);