Figure 4.5:

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

Code for Figure 4.5

Text of the GNU GPL.

main.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
% This file simulates the Batch Reactor Example (3.2.1) from Haseltine
% & Rawlings (2005)
% MHE is implemented to this example and it is shown that it
% performs better than EKF and UKF when a poor prior is used.
mpc = import_mpctools();

% States: x = [Ca Cb Cc]'
% Total pressure is measured (C = RT*[1 1 1])
Nsim = 120;
pars = batchreactor(Nsim);
Nx = pars.Nx;
Ny = pars.Ny;
Nw = pars.Nw;
Nv = pars.Nv;
Nt = 10;
xsim = pars.xsim;
ysim = pars.ysim;

% Pick stage costs.
Q = pars.Q;
R = pars.R;
sig_w = pars.sig_w;
sig_v = pars.sig_v;

lfunc = @(w, v) (w'*w)/sig_w.^2 + (v'*v)/sig_v.^2;
l = mpc.getCasadiFunc(lfunc, [Nw, Nv], {'w', 'v'}, {'l'});

% Now build MHE solver.
mhepar = struct('Pinv', mpctools.spdinv(pars.P), 'x0bar', pars.xhat0);

N = struct('x', Nx, 'w', Nw, 'y', Ny, 't', Nt);

lb = struct('x', zeros(Nx, Nt + 1)); % Concentrations are nonnegative.
ub = struct('x', 10*ones(Nx, Nt + 1)); % Concentrations shouldn't be this high.

buildsolvertime = tic();
solver = mpc.nmhe('f', pars.f, 'l', l, 'h', pars.h, 'y', ysim(:,1:(Nt + 1)), ...
                  'N', N, 'lb', lb, 'ub', ub, 'par', mhepar, ...
                  'priorupdate', 'smoothing', 'verbosity', 0);

% Loop.
xhat = NaN(Nx, Nsim + 1);
yhat = NaN(Ny, Nsim + 1);
tic();
for t = 1:(Nsim + 1)
    % Get new measurement or extend horizon.
    if t > Nt + 1
        solver.newmeasurement(ysim(:,t));
    else
        solver.truncatehorizon(t - 1);
    end

    % Solve MHE problem and save state estimate.
    solver.solve();
    if ~isequal(solver.status, 'Solve_Succeeded')
        warning('Solver failure at time %d!', t);
        break
    end

    solver.saveestimate(); % Stores estimate to history and updates prior.
    xhat(:,t) = solver.history(1).xhat(:,end);
    yhat(:,t) = solver.history(1).yhat(:,end);
end

% Make a plot.
pars.reactorplot(xsim, xhat, ysim, yhat, pars.yclean, pars.Delta);

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