Application and Topics Covered

The application envisioned for this example is automatic lane tracking on a road. We will show how to fit a polynomial to noisy data representing the lane boundary of the road ahead of a vehicle.

The example uses this curve fitting application to illustrate several topics, including:

In many curve fitting applications, the objective is to estimate the coefficients of a low-order polynomial. The polynomial is then used as a model for observed noisy data. For example, if a quadratic polynomial is to be used, there are three coefficients (a, b and c) to estimate:

The polynomial that fits best is defined as the one that minimizes the sum of the squared errors between itself and the noisy data. In order to solve this least squares problem, an overdetermined linear system is obtained and solved. An explicit matrix inverse is not actually required in order to solve the system.

This example will first illustrate some of these points in MATLAB®, and then move to a Simulink® model.

Signal Model for the Road

In order to test the algorithm, a continuously curving road model is used: a sinusoid that is contaminated with additive noise. By varying the frequency of the sinusoid in the model, you can stress the algorithm by different amounts. The following code simulates noisy sensor outputs using our road model:

% Model of roadway
Duration = 2;        % Distance that we look ahead
N = 25;              % Total number of sensors providing estimates of road boundary
Ts = Duration / N;   % Sample interval
FracPeriod = 0.5;    % Fraction of period of sinusoid to match
% y will contain the simulated sensor outputs
y = sin(2*pi* [0:(N-1)]' * (FracPeriod/N)) + sqrt(0.3) * randn(N,1);

Solving a Linear System Using Matrix Factorization

In this example, the unknowns are the coefficients of each term in the polynomial. Since the polynomial that we will use as a model always starts from our current position on the road, the constant term in the polynomial is assumed to be zero and we only have to estimate the coefficients for the linear and higher-order terms. We will set up a matrix equation Ax=y such that

We will solve the equation using the QR factorization of A as follows:

and

where pinv() represents pseudo-inverse. Given the matrix A, the following code can be used to implement a solution of this matrix equation. Factoring A allows for easier solution of the system.

[Q, R] = qr(A);
z = Q' * y;
x = R \ z;
yhat = A * x;
plot(0:N-1,y,'k',0:N-1,yhat,'rs')

For the sake of completeness we note that the Vandermonde matrix A can be formed using the following code:

Npoly = 3; % Order of polynomial to use in fitting
v = [0:Ts:((N-1)*Ts)]';
A = zeros(length(v), Npoly);
for i = Npoly : -1 : 1
    A(:,i) = v.^i;
end

Since A is constant, so are Q and R. They can be precomputed. Therefore the only computation required as new sensor data is obtained is x=R\(Q'*y).

Once we have estimates of the polynomial coefficients, we can reconstruct the polynomial with whatever granularity we desire - we are not limited to reconstructing only at the points where we originally had data:

Moving to Simulink® and Preparing for Fixed-Point Implementation

Next we reconstruct this problem and its solution in the Simulink environment. The ultimate deliverable that we have in mind is a fixed-point implementation of the run-time portion of the algorithm, suitable for deployment in an embedded environment.

The model that implements this curve-fitting application is conceptually divided into four parts:

The example model is parameterized in the same manner as the MATLAB code above. To see the model's parameters, go to the Model Workspace, which is available from the Model Explorer. Select MODELING > Model Explorer and Model Workspace (in the middle pane) and try the following:

As it is our intent to implement this model using fixed-point arithmetic, one of the first questions must be whether the necessary components support fixed-point. The QR Factorization block in the DSP System Toolbox™ does not currently support fixed-point data types, so are we stuck? The answer is no. The matrix that is to be factored does not depend on run-time data. It only depends on things like the order of the polynomial, the sample time, and the observation window. Thus, we assume that the A matrix will be factored "offline". Offline computations can be modeled in Simulink using invariant signals.

A signal in Simulink becomes invariant by setting the sample time of the relevant source block to Inf, and turning on the Inline parameters optimization on the Configuration Parameters dialog. This model has sample time colors turned on (Display > Sample Time > Colors). The magenta part of the model has infinite sample time - its output is computed once, before the model begins to simulate, and is then used by the rest of the model during run-time. The offline computations that it represents can actually be computed in floating point and then their output simply converted to fixed point for use by the rest of the model at run-time.

Running the model produces output similar to that of the MATLAB code above, except that it runs continually. A snapshot of the model's output is shown below:

Workflow for Floating-Point-to-Fixed-Point Conversion

We will now work toward reimplementing this system using fixed-point data. Note that even if a fixed-point system is the ultimate deliverable, it is usually desirable to start by implementing it in floating point and then converting that functional implementation to fixed point. In the remainder of this example, we will illustrate this workflow by converting the model above from floating point to fixed point. The Fixed-Point Tool in Simulink facilitates this workflow. Open this tool from your model by selecting Fixed Point Tool from the APPS menu. The primary functionalities that we will be using are data type override and autoscaling. Data type override is a convenient way to switch an entire model or subsystem between floating-point and fixed-point operation. Autoscaling automatically suggests desirable scaling for the various fixed-point quantities in a model or subsystem. Scaling specifies the location of the binary point within the specified word size for each quantity.

Using Data Type Override with Fixed-Point Models

It is often convenient to move back and forth between floating-point and fixed-point data types as you are optimizing a model for fixed-point behavior. It is generally desirable for the fixed-point behavior to match the floating-point behavior as closely as possible. To see the effect of data type override with the example model, try the following:

Proposing Optimized Fixed-Point Settings and Autoscaling the Model

Once you run the model with fixed-point data, you can observe that the model is not properly scaled. Let's try to fix that.

Configuration of fixed-point systems is a challenging task. Each fixed-point quantity must have its signedness, word length, and fraction length (binary point location) set. In many cases, the word length and signedness are known, but the binary point must be located. In DSP System Toolbox, we use the quantity fraction length to set binary point location. Fraction length is the number of bits to the right of the binary point.

In order to set the scaling you can follow the general procedure outlined in the "Workflow for Floating-Point-to-Fixed-Point Conversion" section above. Specifically,