clear all
close all
clc
format long e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PROBLEM CONSTANTS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k2 = 3.70;
m = 15;
h = 5.4;
w = sqrt((k_true+k2)/m);
xb=5;
t=(0:0.1:60).';
Npoints=length(t)
std_dev_rho = 30e-3;
std_dev_rate = 3e-3;
file1 = load('../observables_range.txt');
file2 = load('../observables_rate.txt');
rho_oss = file1(:,2);
rho_rate_oss = file2(:,2);
for i=1:length(t)
y_oss(2*i-1,1) = rho_oss(i);
y_oss(2*i,1) = rho_rate_oss(i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A PRIORI VALUES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0_est = 7;
v0_est = 0.5;
k_est = 2.3;
sigx0_ap = 30;
sigv0_ap = 3;
sigk_ap = 0.3;
P0 = [sigx0_ap^2 0 0; 0 sigx0d_ap^2 0; 0 0 sigk_ap^2];
dx0_ap = [0;0;0];
sol_est = [ x0_est; x0d_est; k_est];
for Niter = 1:10
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INITIALIZATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define variables to be used in the current iteration
x0_iter = x0_est;
v0_iter = v0_est;
k_iter = k_est;
w=sqrt((k_iter+k2)/m);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% COMPUTED OBSERVABLES GENERATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% integration of the trajectory
% compute the computed observables
% store them in a vector
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DATA WEIGHTING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Weight matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RESIDUALS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute residuals and their statistics (Mean, RMS, SOS)
% Range residuals vector name = y_rho
% Range rate residuals vector name = y_rate
figure(2*counter-1);
plot(t,y_rho,'x')
string=sprintf('Range residuals: Iter %i\nM = %9.3e R= %9.3e SOS=%d\n',[counter-1,M_rho,RMS_rho,SOS_rho]);
title(string,'fontsize',14)
xlabel('time (s)','fontsize',12)
ylabel('residuals (m)','fontsize',12)
set(gca,'fontsize',11)
figure(2*counter)
plot(t,y_rate,'x')
string=sprintf('Rate residuals: Iter %i\nM = %9.3e R= %9.3e SOS=%d\n',[counter-1,M_rate,RMS_rate,SOS_rate]);
title(string,'fontsize',14)
xlabel('time (s)','fontsize',12)
ylabel('residuals (m/s)','fontsize',12)
set(gca,'fontsize',11)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% H MATRIX COMPUTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% for each time compute H_tilde, state transition matrix, H
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LEAST SQUARES ESTIMATOR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Evaluate covariance
% Evaluate differential correction
% Update the a priori difference
dx0_ap = dx0_ap - dx0_est;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% UPDATING SOLUTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Update the solution
sol_est = sol_est + dx0_est;
x0_est = sol_est(1);
v0_est = sol_est(2);
k_est = sol_est(3);
x0_sig = sqrt(Pcov(1,1));
v0_sig = sqrt(Pcov(2,2));
k_sig = sqrt(Pcov(3,3));
% Extract the Correlation Matrix
end
% Propagate the covariance for x and v