Robust.m

function [x1_rms_robu, x1_rms_opt, x2_rms_robu, x2_rms_opt] = Robust(alpha, beta)

% Simulate a robust Kalman filter.
% INPUTS:
%   alpha = relative change in Q from nominal
%   beta = relative change in R from nominal

K_optimal = [0.02; 0.002]; % optimal Kalman gain for this problem
duration = 100; % simulation length
dt = 0.1; % step size
a = [1 dt;0 1]; % system matrix
b = [dt^2;dt]; % input matrix
B_w = eye(2); % noise input matrix
c = [1 0]; % measurement matrix

rho1 = 0.5; % relative weight on nominal Kalman filter performance
rho2 = 0.5; % relative weight on robustness
randn('state',sum(100*clock));
measnoise = 10; % std dev of measurement noise
accelnoise = 0.2; % std dev of process noise
R = measnoise^2; %measurement noise covariance
Q = accelnoise^2*[dt^4/4 dt^3/2;dt^3/2 dt^2]; %process noise covariance
x = [0; 0]; % initial state
xhat = x; % initial state estimate
xhat_new = x; % initial robust state estimate

x1hatnew=0; x2hatnew=0;
x1=0; x2=0;
x1hat=0; x2hat=0;
epsilon = 0.01;
zero_temp = eps*eye(2,2);
JArray = [];
K = K_optimal;

J = inf;
% Minimize the robust Kalman filter cost function
while (1 == 1)   
    X_1 = DARE ( (a-K*c*a)', zeros(2,2), (B_w-K*c*B_w)*Q*(B_w-K*c*B_w)', zero_temp, zeros(2,2), eye(2) );
    X_2 = DARE ( (a-K*c*a)', zeros(2,2), (K*R*K'), zero_temp, zeros(2,2), eye(2) );
    J_old = J;
    Jnominal = trace(X_1)+trace(X_2);
    Jrobust = trace(X_1)^2+trace(X_2)^2;
    J = rho1*Jnominal + rho2*Jrobust;
    disp(['Jnominal = ', num2str(Jnominal), ', Jrobust = ', num2str(Jrobust), ', J = ',num2str(J)]);
    JArray = [JArray J];
    if (J > 0.999*J_old ) 
        break; % convergence
    end;
    % Partial of J with respect to X1 and X2
    par_J_X1 = rho1*eye(2)+2*rho2*trace(X_1)*eye(2);
    par_J_X2 = rho1*eye(2)+2*rho2*trace(X_2)*eye(2);
    % Change K so that the partial of X1 and X2 w/r to K can be computed
    % numerically.
    D_K_1 = [K(1,1)*(1 + epsilon);K(2,1)];
    D_K_2 = [K(1,1);K(2,1)*(1 + epsilon)];
    % PARTIAL OF X_1 w/r to K
    X_1_new_K1 = DARE ( (a-D_K_1*c*a)', zeros(2,2), (B_w-D_K_1*c*B_w)*Q*(B_w-D_K_1*c*B_w)', zero_temp, zeros(2,2), eye(2) );
    X_1_delta_K1 = X_1 -X_1_new_K1;
    deltaX_1_K1 = [X_1_delta_K1(1,1) X_1_delta_K1(1,2) X_1_delta_K1(2,1) X_1_delta_K1(2,2)];
    X_1_new_K2 = DARE ( (a-D_K_2*c*a)', zeros(2,2), (B_w-D_K_2*c*B_w)*Q*(B_w-D_K_2*c*B_w)', zero_temp, zeros(2,2), eye(2) );
    X_1_delta_K2 = X_1 -X_1_new_K2;
    deltaX_1_K2 = [X_1_delta_K2(1,1) X_1_delta_K2(1,2) X_1_delta_K2(2,1) X_1_delta_K2(2,2)];
    DeltaX_1_K = [deltaX_1_K1;deltaX_1_K2];
    % Partial of X_2 w/r to K
    X_2_new_K1 = DARE ( (a-D_K_1*c*a)', zeros(2,2), (D_K_1*R*D_K_1'), zero_temp, zeros(2,2), eye(2) );  
    X_2_delta_K1 = X_2 -X_2_new_K1;
    deltaX_2_K1 = [X_2_delta_K1(1,1) X_2_delta_K1(1,2) X_2_delta_K1(2,1) X_2_delta_K1(2,2)];
    X_2_new_K2 = DARE ( (a-D_K_2*c*a)', zeros(2,2), (D_K_2*R*D_K_2'), zero_temp, zeros(2,2), eye(2) );
    X_2_delta_K2 = X_2 -X_2_new_K2;
    deltaX_2_K2 = [X_2_delta_K2(1,1) X_2_delta_K2(1,2) X_2_delta_K2(2,1) X_2_delta_K2(2,2)];
    DeltaX_2_K = [deltaX_2_K1;deltaX_2_K2];
    % Partial of J w/r to K 
    temp1 = par_J_X1(1,1)*DeltaX_1_K(1,1)+par_J_X1(1,2)*DeltaX_1_K(1,2)+par_J_X1(2,1)*DeltaX_1_K(1,3)+...
        par_J_X1(2,2)*DeltaX_1_K(1,4);
    temp2 = par_J_X1(1,1)*DeltaX_1_K(2,1)+par_J_X1(1,2)*DeltaX_1_K(2,2)+par_J_X1(2,1)*DeltaX_1_K(2,3)+...
        par_J_X1(2,2)*DeltaX_1_K(2,4);
    temp3 = par_J_X2(1,1)*DeltaX_2_K(1,1)+par_J_X2(1,2)*DeltaX_2_K(1,2)+par_J_X2(2,1)*DeltaX_2_K(1,3)+...
        par_J_X2(2,2)*DeltaX_2_K(1,4);
    temp4 = par_J_X2(1,1)*DeltaX_2_K(2,1)+par_J_X2(1,2)*DeltaX_2_K(2,2)+par_J_X2(2,1)*DeltaX_2_K(2,3)+...
        par_J_X2(2,2)*DeltaX_2_K(2,4);
    Delta_J_K = [temp1;temp2]+[temp3;temp4];
    % Use gradient descent to compute a new K
    K_new = K + epsilon*Delta_J_K;
    K = K_new;
end

for t = 0 : dt : duration
    u = 1; % control input
    ProcessNoise = (1+alpha)*accelnoise*[(dt^2/2)*randn; dt*randn];
    x = a*x+b*u+ProcessNoise;
    x1 = [x1 x(1)];
    x2 = [x2 x(2)];
    MeasNoise = (1+beta)*measnoise*randn;
    y = c*x+MeasNoise;
    xhat = a*xhat+b*u;
    xhat_new = a*xhat_new+b*u;
    Inn1 = y-c*xhat;
    Inn2 = y-c*xhat_new;
    % Standard Kalman filter estimate
    xhat = xhat+K_optimal*Inn1;
    x1hat = [x1hat xhat(1)];
    x2hat = [x2hat xhat(2)];
    % Robust Kalman filter estimate
    xhat_new = xhat_new+K_new*Inn2;
    x1hatnew = [x1hatnew xhat_new(1)];
    x2hatnew = [x2hatnew xhat_new(2)];
end

x1_sqr = (x1-x1hatnew).^2;
x1_rms_robu = (mean(x1_sqr))^0.5;
x1_sqr_op = (x1-x1hat).^2;
x1_rms_opt = (mean(x1_sqr_op))^0.5;
disp(['RMS x1 error = ',num2str(x1_rms_robu),' (robust), ',num2str(x1_rms_opt),' (optimal)']);

x2_sqr = (x2-x2hatnew).^2;
x2_rms_robu = (mean(x2_sqr))^0.5;
x2_sqr_op = (x2-x2hat).^2;
x2_rms_opt = (mean(x2_sqr_op))^0.5;
disp(['RMS x2 error = ',num2str(x2_rms_robu),' (robust), ',num2str(x2_rms_opt),' (optimal)']);