clear;clc;

A = [1.1269 -0.4940 0.1129

1.0000 0 0
0 1.0000 0];
B = [-0.3832
0.5919
0.5191];
C = [1 0 0];
Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y');
Q = 1; R = 1;
[kalmf,L,P,M] = kalman(Plant,Q,R);
a = A;
b = [B B 0*B];
c = [C;C];
d = [0 0 0;0 0 1];
P = ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'});
sys = parallel(P,kalmf,1,1,[],[])
% Close loop around input #4 and output #2
SimModel = feedback(sys,1,4,2,1)
% Delete yv from I/O list
SimModel = SimModel([1 3],[1 2 3])

t = [0:100]';
u = sin(t/5);

n = length(t)
randn('seed',0)
w = sqrt(Q)*randn(n,1);
v = sqrt(R)*randn(n,1);

[out,x] = lsim(SimModel,[w,v,u]);

y = out(:,1); % true response

ye = out(:,2); % filtered response
yv = y + v; % measured response

subplot(211), plot(t,y,'--',t,ye,'-'),
xlabel('No. of samples'), ylabel('Output')
title('Kalman filter response')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-'),
xlabel('No. of samples'), ylabel('Error')

MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr);
EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr);
%%%% Constant Velocity Model Kalman Filter Simulation %%%%

%==================================================================
========

clear all; close all; clc;

%% Initial condition
ts = 1; % Sampling time
t = [0:ts:100];
T = length(t);
%% Initial state
x = [0 40 0 20]';
x_hat = [0 0 0 0]';
%% Process noise covariance
q=5
Q = q*eye(2);

%% Measurement noise covariance

r=5
R = r*eye(2);

%% Process and measurement noise

w = sqrt(Q)*randn(2,T); % Process noise
v = sqrt(R)*randn(2,T); % Measurement noise
%% Estimate error covariance initialization
p = 5;
P(:,:,1) = p*eye(4);

%===================================================================
=======

%% Continuous-time state space model

%{
x_dot(t) = Ax(t)+Bu(t)
z(t) = Cx(t)+Dn(t)
%}
A = [0 1 0 0;
0 0 0 0;
0 0 0 1;
0 0 0 0];
B = [0 0;
1 0;
0 0;
0 1];
C = [1 0 0 0;
0 0 1 0];
D = [1 0;
0 1];

%% Discrete-time state space model

%{
x(k+1) = Fx(k)+Gw(k)
z(k) = Hx(k)+Iv(k)
Continuous to discrete form by zoh
%}
sysc = ss(A,B,C,D);
sysd = c2d(sysc, ts, 'zoh');
[F G H I] = ssdata(sysd);
%% Practice state of target
for i = 1:T-1
x(:,i+1) = F*x(:,i);
end
x = x+G*w; % State variable with noise
z = H*x+I*v; % Measurement value with noise

%===================================================================
=======

%%% Kalman Filter

for i = 1:T-1

%% Prediction phase
x_hat(:,i+1) = F*x_hat(:,i);
% State estimate predict
P(:,:,i+1) = F*P(:,:,i)*F'+G*Q*G';
% Tracking error covariance predict
P_predicted(:,:,i+1) = P(:,:,i+1);

%% Kalman gain
K = P(:,:,i+1)*H'*inv(H*P(:,:,i+1)*H'+R);

%% Updata step
x_hat(:,i+1) = x_hat(:,i+1)+K*(z(:,i+1)-H*x_hat(:,i+1));
% State estimate update
P(:,:,i+1) = P(:,:,i+1)-K*H*P(:,:,i+1);
% Tracking error covariance update
P_updated(:,:,i+1) = P(:,:,i+1);

end

%===================================================================
=======
%% Estimate error
x_error = x-x_hat;
%% Graph 1 practical and tracking position
figure(1)
plot(x(1,:),x(3,:),'r');
hold on;
plot(x_hat(1,:),x_hat(3,:),'g.');
title('2D Target Position')
legend('Practical Position','Tracking Position')
xlabel('X axis [m]')
ylabel('Y axis [m]')
hold off;
%% Graph 2
figure(2)
plot(t,x(1,:)),grid on;
hold on;
plot(t,x_hat(1,:),'r'),grid on;
title('Practical and Tracking Position on X axis')
legend('Practical Position','Tracking Position')
xlabel('Time [sec]')
ylabel('Position [m]')
hold off;
%% Graph 3
figure(3)
plot(t,x_error(1,:)),grid on;
title('Position Error on X axis')
xlabel('Time [sec]')
ylabel('Position RMSE [m]')
hold off;
%% Graph 4
figure(4)
plot(t,x(2,:)),grid on;
hold on;
plot(t,x_hat(2,:),'r'),grid on;
title('Practical and Tracking Velocity on X axis')
legend('Practical Velocity','Tracking Velocity')
xlabel('Time [sec]')
ylabel('Velocity [m/sec]')
hold off;
%% Graph 5
figure(5)
plot(t,x_error(2,:)),grid on;
title('Velocity Error on X axis')
xlabel('Time [sec]')
ylabel('Velocity RMSE [m/sec]') ;hold off;
clc;

clear all;

n=input('n=');

N=input('N=');

sigma=input('sigma=');

Q=input('Q=');

g=input('g=');

tetahat=zeros(n,1);

sigma=sigma*eye(n,n);

t=1;

y=0;

% a=zeros(10,10);

% a(6,6)=Q;

a=eye(10,10);

a(6,6)=Q;

Q=a*eye(10,10);

while(t<=N)

u_trn(t,1)=rand;

tans=(1+tansig (g * (t - 375)));

si = sin (0.01*i);

y_trn(t,1)=1+3*(si*u_trn(t,1)^2)+(tans*u_trn(t,1)^3)+2*(tanh(10*t)*u_trn(t,1)^5)+u_trn(t,1)^8;

for k=1:n

x(k,1)=u_trn(t,1)^(k-1);

end

P_err(t,1) = y_trn(t,1)-(x'*tetahat);
 sigma=sigma-(sigma*x*x'*sigma)/(1+x'*sigma*x)+Q;

tetahat = tetahat+(sigma-Q)*x*P_err(t,1);

yhat_trn(t,1)=x'*tetahat;

t=t+1;

end

tetahat

err_train=y_trn(0.7*N,1)-yhat_trn(0.7*N,1)

%Test%

u_test=rand(0.3*N,1);

for k=1:0.3*N

y_test(k,1)=1+3*(si*u_test(k,1)^2)+(tans*u_test(k,1)^3)+2*(tanh(10*t)*u_test(k,1)^5)+u_test(k,1)^8;

for k2=1:n

x_test(k,k2)=u_test(k,1)^(k2-1);

end

end

yhat_test=x_test*tetahat;

err_test=y_test(0.3*N,1)-yhat_test(0.3*N,1)

plot(u_test,y_test,'.b')

hold on

plot(u_test,yhat_test,'.red')

title('Diagrams')
xlabel('U')

legend('Real output','Estimated output')

grid on

figure(2)

err_train=y_trn-yhat_trn;

err_test=y_test-yhat_test;

plot(u_trn,err_train,'.green',u_test,err_test,'.black')

title('Diagrams')

xlabel('U')

legend('Train Error','Test Error')

grid on

figure(3)

plot(u_trn,P_err,'.b')

hold on

plot(u_trn,err_train,'.red')

title('Diagrams')

xlabel('U')

legend('prediction error','train error')

grid on

cov_yhat=cov(yhat_test);

W=diag(cov_yhat);

error_bar_p=yhat_test'+sqrt(W);

error_bar_n=yhat_test'-sqrt(W);
figure(4)

plot(u_test,yhat_test,'.y',u_test,error_bar_p,'.r',u_test,error_bar_n,'.g')

title('Diagrams')

xlabel('U')

legend('ErrorBar')

grid on

figure(5)

err_test=y_test-yhat_test;

plot(err_test,'.black')

title('Diagrams')

xlabel('U')

legend('Test Error')

grid on
clear;

clc;

K=1;

M=2; %sensor numbers

N=500; % Number of time steps. `

Q=1/3000; % Process noise variance.

R=0.005; % Measurement noise variance.

SNR=13;

v(1,:)=sqrt(Q)*randn(1,N);

for m=1:M

w(m,:)=sqrt(R)*randn(1,N);

end

x(1,1)=0.5;

for t=2:N

x(1,t)=x(1,t-1)+v(1,t-1);

end

a=zeros(1,N);

a(1,:)=sqrt(10^(SNR/10)*R)*randn(1,N);

%*************define measurements with noises y(M,N)*************

for t=1:N

for m=1:M

%defind lth H matric at time t

if ((m-1)*x(1,t))==0

H(m,t)=(sin((-(m-1)*x(1,t))*pi)+10^(-20))/((-(m-
1)*x(1,t))*pi+10^(-20));

else
 H(m,t)=(sin((-(m-1)*x(1,t))*pi))/((-(m-1)*x(1,t))*pi);

end

end

y(:,t)=H(:,t)*a(1,t);

end

y=y+w;

%EKF

%��H����������Ի������ȶ�H���Ա���x��

for t=1:N

for m=1:M

if ((m-1)*x(1,t))==0

HH(m,t)=cos((-m+1)*x(1,t)*pi)*(-m+1)*pi/((-m+1)*x(1,t)*pi+10^(-20))-(sin((-
m+1)*x(1,t)*pi)+10^(-20))/((-m+1)*x(1,t)*pi+10^(-20))^2*(-m+1)*pi*a(1,t);

else

HH(m,t)=cos((-m+1)*x(1,t)*pi)/x(1,t)-sin((-m+1)*x(1,t)*pi)/(-m+1)/x(1,t)^2/pi*a(1,t) ;

end

end

end

xx(1,1)=mean(x(1,1)); %x���Ƴ�ֵ

PP(1,1)=var(x(1,1)); %Ԥ��‫ֵ����ؾ����� ̬״‬

for t=1:N-1

for m=1:M

x1(1,t+1)=xx(1,t)+v(1,t);

P(1,t+1)=PP(1,t)+Q;

S(m,t+1)=P(1,t+1)*(HH(m,t+1))'*inv(HH(m,t+1)*P(1,t+1)*(HH(m,t+1))'+R) ;
%����������
 PP(1,t+1)=P(1,t+1)-S(m,t+1)*HH(m,t+1)*P(1,t+1);

xx(1,t+1)=x1(1,t+1)+S(m,t+1)*w(m,t+1);

end

end

figure(1)

plot(1:N,x(1,:),'b',1:N,xx(1,:),'r');

legend('True value','EKF Estimation');

ylabel('State estimation and true value','fontsize',15);

xlabel('Time step','fontsize',15);

figure(2)

plot(1:N,x(1:N)-xx(1:N));

ylabel('Error','fontsize',15);

xlabel('Time step','fontsize',15);