%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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_0908.txt %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load A_hat.mat
load B_hat.mat
load C_hat.mat
load in_out_ind.mat
u = min_avg_0908(:,in_out_ind(1));
y = min_avg_0908(:,in_out_ind(2));
dt = 1; m = length(u)
t = [1:1:m]';
RError = (0.01)^2;
est = zeros(m,1);
ymeas = y + sqrt(RError).*randn(m,1);
h = [C_hat];
phi = A_hat;
B = B_hat;
p0 = 1000*eye(1);
p=p0;
pcov = diag(p0)';
q = .005; %.005
sysdt = ss(A_hat,B_hat,C_hat,[0],1);
sysc = d2c(sysdt)
x0 = 0;
[yy,tt,xx] = lsim(sysdt,u,t,x0);
est = C_hat*xx;
% [T,X] = ode45(@turbine,[t],est',[],u);
%
% X = -5.857*X;
% plot(X)
%
% break
Est(1) = 0 %
for i=1:m-1;
% Kalman Update Equations
kgain = p*h'*inv(h*p*h'+ RError);
p = (eye(1)-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;
Est = h.*Est;
figure(2)
plot(t(1:m-1),Est(1:m-1),'r--',t(1:m-1),y(1:m-1),'--')
ylabel('RPM (rev/min)')
xlabel('Time (mins)')
title('RPM vs. Time')
xlim([2100 2300])
xlim([-100 t(end)+100])
figure(3)
plot(t(1:m-1),(y(1:m-1)-(Est(1:m-1))'),t(1:m-1),sigma3(1:m-1),'r--',t(1:m-1),-sigma3(1:m-1),'r--')
ylabel('RPM Estimate Errors')
xlabel('Time (mins)')
title('Errors and 3\sigma Bounds vs. Time')
ylim([-1 1])
xlim([-100 t(end)+100])
