LKF_Windspd_POWERReg_4SS.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sean Semper                                         System Identification
%/ Sumit Tripathi
% Project Code 2                                                    MAE 566
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Last Modified: 04/24/09
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kalman Filter for Windspeed -- RPM Relationship Estimation of RPM
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all
clear
clc
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% time%	nobs	avg_rpm 	avg_powero	avg_powerr	avg_windspeed      %
load min_avg_1009.txt                                                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load A_hat.mat 
load B_hat.mat
load C_hat.mat 
load in_out_ind.mat

u = min_avg_1009(1:end,in_out_ind(1));
y = min_avg_1009(1:end,in_out_ind(2));
dt = 1; m = length(u)
t = [1:1:m]';

RError = (0.01)^2;

est = zeros(m,4);

ymeas = y + sqrt(RError).*randn(m,1);
h = [C_hat];
phi = A_hat;
B = B_hat;

p0 = 1e6*eye(4);   
p=p0; pcov = zeros(m,4);                               
pcov = diag(p0)';
q = eye(4); 
q(1,1) = [19]; q(2,2) = [0.00001]; q(3,3) = [.00001]; q(4,4) = [.00001];
sysdt = ss(A_hat,B_hat,C_hat,[0],1);

sysc = d2c(sysdt)

% figure(1)
% impulse(sysc)


x0 = [0 0 0 0];
[yy,tt,xx] = lsim(sysdt,u,t,x0);  
 est = C_hat*xx';
figure
plot(t,yy,t,C_hat(1).*xx(:,1),'r-.')
% break


Est(1,:) = [0 0 0 0]; 

for i=1:m-1; 
    
    % Kalman Update Equations
    kgain = p*h'*inv(h*p*h'+ RError);
    p = (eye(4)-kgain*h)*p;
    Est(i,:) = Est(i,:) + (kgain*(ymeas(i,:)-h*Est(i,:)'))';
    
    % Propagation Equations
    Est(i+1,:) = Est(i,:)*phi;
    p = phi*p*phi'+ q;
    pcov(i+1,:) = diag(p);
    
end
 sigma3 = (pcov.^0.5)*3;
%  break
Est(:,1)=Est(:,1).*h(1)';

figure(2)
plot(t(1:m-1),Est(1:m-1,1),'r--',t(1:m-1),y(1:m-1),'--')
ylabel('Power Output (Watts)')
xlabel('Time (mins)')
title({'Power Output (Watts) vs. Time';'State-Space ID with March 2009 and Oct 2008 Validation'})
xlim([1800 2000])

figure(3)
% subplot(211)
plot(t(1:m-1),(y(1:m-1)-Est(1:m-1,1)),t(1:m-1),sigma3(1:m-1,1),'r--',t(1:m-1),-sigma3(1:m-1,1),'r--')
ylim([-80 80])
xlim([-100 t(end)+100])
ylabel('Power Output (Watts) Estimate Errors')
xlabel('Time (mins)')
title('Errors and 3\sigma Bounds vs. Time')