Figure 4.3:

Evolution of the state (solid line) and EKF state estimate (dashed line).

Code for Figure 4.3

Text of the GNU GPL.

main.m

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
% This file simulates the Batch Reactor Example (3.2.1) from Haseltine
% & Rawlings (2005)
% EKF is implemented to this example and it is shown that it
% fails when a poor prior is used.
data = struct();

% States: x = [Ca Cb Cc]'
% Total pressure is measured (C = RT*[1 1 1])

% Simulate.
fprintf('Simulating without clipping.\n');
data.noclip = doekf(120, false());
fprintf('Simulating with clipping.\n');
data.yesclip = doekf(600, true());

batchreactor.m

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
function pars = batchreactor(Nsim)
% pars = batchreactor(Nsim)
%
% Returns a struct of batch reactor parameters to avoid duplication across
% MHE, EKF, and UKF scripts.
narginchk(1, 1);
mpc = import_mpctools();

% Sizes.
Delta = 0.25;
Nx = 3;
Ny = 1;
Nw = Nx;
Nv = Ny;

% Random variable standard deviations.
sig_v = 0.25; % Measurement noise.
sig_w = 0.001; % State noise.
sig_p = 0.5; % Prior.

P = sig_p.^2*eye(Nx);
Q = sig_w.^2*eye(Nw);
R = sig_v.^2*eye(Ny);

% Get model functions.
plant = mpc.getCasadiIntegrator(@batch_model, Delta, [Nx], {'x'});
frk4 = mpc.getCasadiFunc(@(x) batch_model(x), [Nx], {'x'}, {'frk4'}, ...
                         'rk4', true(), 'Delta', Delta, 'M', 4);
f = mpc.getCasadiFunc(@(x, w) frk4(x) + w, [Nx, Nw], {'x', 'w'}, {'f'});
h = mpc.getCasadiFunc(@measurement, [Nx], {'x'}, {'h'});

% Choose prior.
xhat0 = [1; 0; 4];
x0 = [0.5; 0.05; 0.0];

% Simulate the system to get data.
randn('state', 0);
w = sig_w*randn(Nw, Nsim);
v = sig_v*randn(Nv, Nsim + 1);

xsim = NaN(Nx, Nsim + 1);
xsim(:,1) = x0;

ysim = NaN(Ny, Nsim + 1);
yclean = NaN(Ny, Nsim + 1);

for t = 1:(Nsim + 1)
    xsim(:,t) = max(xsim(:,t), 0); % Make sure concentration is nonnegative.
    yclean(:,t) = full(h(xsim(:,t)));
    ysim(:,t) = yclean(:,t) + v(:,t);

    if t <= Nsim
        xsim(:,t + 1) = full(plant(xsim(:,t))) + w(:,t);
    end
end
t = (0:Nsim)*Delta;

% Get plotting function for convenience.
reactorplot = @reactorplot;

% Package everybody up. Note that this is a quick and dirty approach.
varnames = who();
pars = struct();
for i = 1:length(varnames);
    v = varnames{i};
    pars.(v) = eval(v);
end

end%function

function dxdt = batch_model(x)
    % Nonlinear ODE function.
    k1 = 0.5;
    km1 = 0.05;
    k2 = 0.2;
    km2 = 0.01;

    cA = x(1);
    cB = x(2);
    cC = x(3);

    rate1 = k1*cA - km1*cB*cC;
    rate2 = k2*cB.^2 - km2*cC;
    dxdt = [
        -rate1;
        rate1 - 2*rate2;
        rate1 + rate2;
    ];
end%function

function y = measurement(x)
    % Linear measurement function.
    RT = 0.0821*400;
    y = RT*sum(x);
end%function

function fig = reactorplot(x, xhat, y, yhat, yclean, Delta)
    % Make a plot of actual and estimated states.
    narginchk(6, 6);
    fig = figure();
    Nx = 3;
    Nt = size(x, 2) - 1;
    t = (0:Nt)*Delta;

    ax = subplot(2, 1, 2);
    hold('on');
    plot(t, yclean, '-k', 'DisplayName', 'Actual');
    plot(t, yclean, '--c', 'DisplayName', 'Measured');
    plot(t, yhat, 'ok', 'DisplayName', 'Estimated');
    ylabel('P');
    xlabel('t');
    legend('Location', 'EastOutside');
    set(ax, 'ylim', [10, 35]);

    ax = subplot(2, 1, 1);
    hold('on');
    colors = {'r', 'b', 'g'};
    names = {'A', 'B', 'C'};
    for i = 1:Nx
        plot(t, x(i,:) + 1e-10, ['-', colors{i}], 'DisplayName', ...
             sprintf('Actual C_%s', names{i}));
        plot(t, xhat(i,:) + 1e-10, ['o', colors{i}], 'DisplayName', ...
             sprintf('Estimated C_%s', names{i}));
    end
    xlabel('t');
    ylabel('c_i');
    legend('Location', 'EastOutside');
    set(ax, 'ylim', [-1, 1.5]);
end%function

doekf.m

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
function data = doekf(Nsim, clipping)
    % data = doekf(Nsim, clipping)
    %
    % Simulate EKF with or without clipping.
    pars = batchreactor(Nsim);
    Nx = pars.Nx;
    Ny = pars.Ny;
    Nw = pars.Nw;

    % Get linearization functions.
    f = pars.f;
    Afunc = f.factory('Afunc', f.name_in(), 'jac:f:x');
    Gfunc = f.factory('Gfunc', f.name_in(), 'jac:f:w');
    h = pars.h;
    Cfunc = h.factory('Cfunc', h.name_in(), 'jac:h:x');

    % Choose simulation parameters.
    Nsim = pars.Nsim;
    t = pars.t;
    x = pars.xsim;
    y = pars.ysim;
    Qw = pars.Q;
    Rv = pars.R;

    % Try ekf.
    xhat = NaN(Nx, Nsim + 1);
    yhat = NaN(Ny, Nsim + 1);
    e = NaN(Ny, Nsim + 1);

    xhat(:,1) = pars.xhat0; % Poor initial guess
    L = cell(Nsim + 1, 1);
    P = cell(Nsim + 1, 1);
    P{1} = pars.P;

    % Loop for EKF State Estimation
    for t = 1:(Nsim + 1)
        % Measurement.
        yhat(:,t) = full(h(xhat(:,t)));
        e(:,t) = y(:,t) - yhat(:,t);

        % Correction
        C = full(Cfunc(xhat(:,t)));
        L{t} = (P{t}*C')/(C*P{t}*C' + Rv); % EKF gain
        P{t} = P{t} - L{t}*C*P{t};

        xhat(:,t) = xhat(:,t) + L{t}*e(:,t);

        % Implementing clipping
        if clipping
            xhat(:,t) = max(xhat(:,t), 0);
        end

        % Advance forecast.
        if t <= Nsim
            w = zeros(Nw, 1);

            A = full(Afunc(xhat(:,t), w));
            G = full(Gfunc(xhat(:,t), w));

            xhat(:,t + 1) = full(f(xhat(:,t), w));
            P{t + 1} = A*P{t}*A' + G*Qw*G';
        end
    end

    % Make a plot.
    pars.reactorplot(x, xhat, y, yhat, pars.yclean, pars.Delta);
    if clipping
        set(gca(), 'yscale', 'log', 'ylim', [1e-4, 1e3]);
    end

    % Form data table.
    data = [pars.t', x', xhat', y', yhat'];