stacked two column charts on top of each other - matlab

I am trying to stacked two column charts that have positive and negative values on top of each other. But with the following codes I get the column charts separate and sometimes one is hidden behind the other. I am posting the script and a sample data so that it can be replicable. I am using online Matlab
retndecm(1:52,1)= [0.3715
0.3590
0.2862
0.1936
0.2177
0.2111
0.2019
0.2012
0.2162
0.2236
0.1874
0.2116
0.2569
0.2384
0.2436
0.2416
0.2590
0.2682
0.2695
0.2832
0.2616
0.2506
0.2548
0.2470
0.2612
0.2588
0.2399
0.2848
0.3023
0.3166
0.3535
0.3328
0.3187
0.3365
0.3313
0.3417
0.3411
0.4013
0.3717
0.3574
0.3288
0.3071
0.3271
0.2802
0.2787
0.2649
0.2619
0.2258
0.2432
0.2314
0.2151
0.2080];
retndecm(1:52,2)=[0.0561
0.0462
0.0783
0.1246
0.1099
0.0921
0.0755
0.0675
0.0582
0.0599
0.0249
0.0137
0.0136
0.0108
0.0052
0.0176
0.0259
0.0183
0.0258
0.0312
0.0383
0.0421
0.0293
0.0376
0.0355
0.0344
0.0341
0.0321
0.0404
0.0289
0.0318
0.0374
0.0283
0.0289
0.0405
0.0321
0.0402
0.0528
0.0791
0.0756
0.0798
0.0892
0.0933
0.0978
0.1073
0.1194
0.1172
0.1117
0.1174
0.0960
0.1041
0.1178];
retndecm(1:52,3) = [-0.1152
-0.0963
-0.1035
-0.1144
-0.1309
-0.1289
-0.1337
-0.1347
-0.1272
-0.1289
-0.1361
-0.1176
-0.1057
-0.1125
-0.1068
-0.1119
-0.1142
-0.1188
-0.1163
-0.1115
-0.1098
-0.1145
-0.1110
-0.1046
-0.0953
-0.0985
-0.1044
-0.0967
-0.0964
-0.0932
-0.0959
-0.0938
-0.0930
-0.0843
-0.0815
-0.0673
-0.0609
-0.0591
-0.0464
-0.0736
-0.0681
-0.0691
-0.0612
-0.0758
-0.0658
-0.0690
-0.0736
-0.0774
-0.0731
-0.0762
-0.0711
-0.0754];
ndfspi(1:52,1)= [0.6025
0.6024
0.6028
0.6024
0.6030
0.6028
0.6026
0.6031
0.6031
0.6031
0.6029
0.6029
0.6010
0.6030
0.6033
0.6025
0.6034
0.6021
0.6014
0.6027
0.6031
0.6032
0.6030
0.6032
0.6029
0.6025
0.6024
0.6029
0.6026
0.6011
0.6025
0.6013
0.6025
0.6028
0.5989
0.6025
0.6028
0.6035
0.6035
0.6033
0.6031
0.6030
0.6033
0.6027
0.6030
0.6031
0.6033
0.6029
0.6031
0.6027
0.6026
0.6029];
nfundbeta = ndfspi(1:52,1)-1;
%% figures
set(groot,'DefaultFigureColormap',jet);
r=figure;
hold on;
M=retndecm(:,1)';
t = datetime(2001,1,5) + calweeks(1:52);
h = bar(t, retndecm(:,1),.99,"stacked");
set(h, 'FaceColor', 'flat');
h.CData = ndfspi(:,1)';
caxis([min(-1) max(1)]);
hold on;
t = datetime(2001,1,5) + calweeks(1:52);
l=bar(t,retndecm(:,2),.99,"stacked");
l.FaceColor = [.5 0 .5];
hold on;
N=retndecm(:,3)';
t = datetime(2001,1,5) + calweeks(1:52);
h = bar(t, retndecm(:,3),.99,"stacked");
set(h, 'FaceColor', 'flat');
h.CData = nfundbeta(:,1)';
caxis([min(-1) max(1)]);
colorbar('eastoutside');
set(gcf, 'PaperSize', [20 10], 'PaperPosition', [0 0 20 10])
set(findall(gcf,'-property','FontSize'),'FontSize',12);
saveas(gcf,'test2', 'pdf');
hold off;

Related

difference between histcounts and imhist matlab

histcounts and imhist are not returning the same values for counts and bin locations.
x = [0.5000, 0.6429, 0.7143, 0.6429, 0.7857, 0.2857, 0.8571, 0.6429,0, 0.7857, 0.9286, 1.0000, 0.1429, 0.8571, 0.2857, 0.8571, 0.5714, 0.0714];
[c1, l1] = histcounts(x, 6)
c1 =
3 2 1 4 3 5
l1 =
0 0.1700 0.3400 0.5100 0.6800 0.8500 1.0200
[c2, l2] = imhist(x, 6)
c2 =
2 3 0 5 6 2
l2 =
0 0.2000 0.4000 0.6000 0.8000 1.0000
What could be the reason for that?
MATLAB
close all;clear all;clc
nbins=[6 16 26 46]
x = [0.5000, 0.6429, 0.7143, 0.6429, 0.7857, 0.2857, 0.8571, ...
0.6429,0, 0.7857, 0.9286, 1.0000, 0.1429, 0.8571, 0.2857, 0.8571, 0.5714, 0.0714];
one can take it from one side
for k=1:1:numel(nbins)
figure(k);
ax=gca;hold on;grid on
[C1, L1] = histcounts(x,nbins(k));
stem(L1(1:end-1),C1);hold on
[C2, L2] = imhist(x,nbins(k));
stem(ax,L2,C2)
end
or from the other, stem graphs not shown, quite similar to the above ones.
for k=1:1:numel(nbins)
figure(k);
ax=gca;hold on;grid on
[C1, L1] = histcounts(x,nbins(k));
stem(L1(2:end),C1);hold on
[C2, L2] = imhist(x,nbins(k));
stem(ax,L2,C2)
end
The point : imhist is a command for images and it applies an offset to all histogram bin locations depending upon the type of image fed in.
imhist doesn't have a cut-off for tiny images, so the sequence x is assumed as image, which it is not.
Read imhist details here.
In particular this table shows such offset

Can anyone help me make a 3d plot from these variables?

I am a kind of newbie in using Matlab and I have a problem of producing a 3d plot using these three variables.
The variables are:
P=[1 0.8 0.6 0.4 0.2];
N=[0.1429 0.2857 0.4286 0.5714 0.7143 0.8571 1.0000];
K =
0.0359 0.0340 0.0315 0.0298 0.0309
0.0700 0.0669 0.0618 0.0602 0.0601
0.1018 0.0961 0.0896 0.0866 0.0897
0.1270 0.1192 0.1152 0.1091 0.1127
0.1444 0.1390 0.1322 0.1235 0.1284
0.1556 0.1509 0.1424 0.1375 0.1419
0.1656 0.1598 0.1536 0.1466 0.1500
In Matlab, K is in the form of K(:,1),(K:2),k(:,3),K(:,4) and K(:,5)
can anyone help me please on how to do it?.
I have copied some sections of the code below:
P=[1 0.8 0.6 0.4 0.2];
LAMBDA = linspace(0,1,8) * c;
for iL = 1:length(LAMBDA)
lambda = LAMBDA(iL);
for iP=1:length(P)
[Pbi,Pbo,Pb,Rhoi,Rhoo,Rho]=Sim_traffic(lambdai(iL),lambdao(iL),mu,ci,co,1-epsi);
P_out_p(iL,iP) = (lambdai(iL).*(1-P_B_i_conv_s(iL)).*inner_outage(iL,iPR) + lambdao(iL).*(1-P_B_o_conv_s(iL)).*out_outage(iL,iPR));
end
end
N=LAMBDA./c;
K=P_out_p(:,:);
You're probably looking to use surf and will need meshgrid as well:
[p,n] = meshgrid(P,N)dimensions are wrong...
surf(p,n,K)
Although looking at the docs, I think you might be able to skip the meshgrid line here:
surf(P,N,K)

subscript indices must be real positive in cummswapcfbytrintree.m

I have essentially copied the code from the Matlab Example file BermudanSwaption.m in an effort to calibrate the Hull White one factor model to some given market data. As I use a subset about 30 or fewer swaptions for my calibration set, I am able to arrive at an interior solution. But when I use a larger set of instruments, I get a weird error:
Below is the code:
ValuationDate = '10-01-2014';
Settle = datenum(ValuationDate);
% Zero rate data is market data, bootstrapped from Bloomberg and Reuters quotes
CurveDates = [735874;
735882;
735906;
735936;
735950;
736040;
736133;
736224;
736314;
736424;
736606;
736788;
736971;
737153;
737336;
737518;
737701;
737884;
738069;
738251;
738433;
738615;
738797;
738979;
739162;
739345;
739528;
739710;
739893;
740075;
740260;
740442;
740624;
740806;
740989;
741171;
741354;
741536;
741719;
741901;
742084;
742269;
742451;
742633;
742815;
742997;
743180;
743362;
743545;
743728;
743911;
744093;
744278;
744460;
744642;
744824;
745006;
745189;
745372;
745554;
745737;
745919;
746102;
746284;
746469;
746651;
746833;
747015;
747198;
747380;
747563;
747745;
747928;
748111;
748296;
748478;
748660;
748842;
749024;
749206;
749389;
749572;
749755;
749937;
750120;
750302;
750487];
ZeroRates = 1.0e-03*[0.0172;
0.0188;
0.0191;
0.0221;
0.0249;
0.0244;
0.0269;
0.0333;
0.0423;
0.0571;
0.0789;
0.1021;
0.1253;
0.1435;
0.1617;
0.1749;
0.1881;
0.1973;
0.2064;
0.2158;
0.2253;
0.2311;
0.2370;
0.2429;
0.2488;
0.2547;
0.2607;
0.2640;
0.2672;
0.2706;
0.2738;
0.2772;
0.2807;
0.2842;
0.2877;
0.2913;
0.2948;
0.2964;
0.2979;
0.2995;
0.3011;
0.3026;
0.3043;
0.3060;
0.3077;
0.3095;
0.3112;
0.3118;
0.3125;
0.3132;
0.3138;
0.3146;
0.3152;
0.3160;
0.3167;
0.3175;
0.3183;
0.3186;
0.3189;
0.3192;
0.3196;
0.3199;
0.3202;
0.3206;
0.3209;
0.3213;
0.3217;
0.3217;
0.3216;
0.3216;
0.3216;
0.3216;
0.3216;
0.3216;
0.3216;
0.3216;
0.3216;
0.3217;
0.3217;
0.3218;
0.3218;
0.3219;
0.3219;
0.3220;
0.3220;
0.3221;
0.3221];
Compounding = 2;
RateSpec = intenvset('Compounding', 2,'ValuationDate', ValuationDate,'StartDates', ValuationDate,'EndDates', CurveDates,'Rates', ZeroRates);
InstrumentMaturity = datenum('12-Sep-2044');
% Swaption Vol data from Bloomberg
SwaptionBlackVol = [ 0.5940 0.5550 0.4450 0.3710 0.3400 0.3110 0.2910 0.2750 0.2630 0.2520 0.2250 0.2140 0.2080 0.2050;
0.5630 0.5470 0.4420 0.3690 0.3360 0.3090 0.2900 0.2740 0.2630 0.2520 0.2260 0.2150 0.2090 0.2060;
0.5760 0.5330 0.4400 0.3730 0.3410 0.3150 0.2970 0.2820 0.2700 0.2590 0.2330 0.2220 0.2170 0.2140;
0.5840 0.5020 0.4240 0.3730 0.3480 0.3240 0.3060 0.2920 0.2810 0.2710 0.2430 0.2300 0.2230 0.2190;
0.5630 0.4750 0.4100 0.3700 0.3450 0.3230 0.3070 0.2940 0.2830 0.2740 0.2470 0.2330 0.2260 0.2210;
0.5510 0.4520 0.3980 0.3660 0.3410 0.3220 0.3070 0.2950 0.2850 0.2760 0.2500 0.2360 0.2290 0.2240;
0.4630 0.4010 0.3660 0.3440 0.3250 0.3100 0.2990 0.2890 0.2790 0.2720 0.2470 0.2320 0.2260 0.2210;
0.4230 0.3750 0.3480 0.3290 0.3140 0.3030 0.2930 0.2840 0.2760 0.2690 0.2420 0.2300 0.2240 0.2190;
0.3700 0.3470 0.3280 0.3110 0.2960 0.2880 0.2800 0.2730 0.2680 0.2620 0.2360 0.2240 0.2190 0.2150;
0.3420 0.3250 0.3100 0.2970 0.2850 0.2770 0.2700 0.2640 0.2590 0.2540 0.2280 0.2180 0.2140 0.2110;
0.3230 0.3010 0.2900 0.2810 0.2720 0.2650 0.2590 0.2540 0.2500 0.2470 0.2230 0.2130 0.2090 0.2060;
0.3010 0.2860 0.2760 0.2670 0.2580 0.2530 0.2480 0.2450 0.2420 0.2390 0.2160 0.2060 0.2030 0.2000;
0.2850 0.2750 0.2650 0.2560 0.2480 0.2440 0.2400 0.2370 0.2350 0.2320 0.2100 0.2000 0.1970 0.1940;
0.2710 0.2600 0.2510 0.2440 0.2380 0.2340 0.2310 0.2290 0.2260 0.2240 0.2040 0.1940 0.1910 0.1890;
0.2580 0.2470 0.2400 0.2350 0.2300 0.2270 0.2240 0.2210 0.2190 0.2170 0.1980 0.1890 0.1860 0.1840;
0.2460 0.2370 0.2320 0.2270 0.2240 0.2210 0.2180 0.2150 0.2130 0.2110 0.1980 0.1840 0.1820 0.1800;
0.2040 0.1980 0.1950 0.1920 0.1900 0.1890 0.1890 0.1880 0.1880 0.1870 0.1720 0.1660 0.1640 0.1620;
0.1790 0.1750 0.1740 0.1730 0.1730 0.1710 0.1710 0.1700 0.1690 0.1690 0.1530 0.1510 0.1500 0.1480;
0.1650 0.1650 0.1660 0.1670 0.1680 0.1670 0.1670 0.1680 0.1680 0.1680 0.1550 0.1580 0.1560 0.1530;
0.1530 0.1570 0.1590 0.1620 0.1640 0.1650 0.1660 0.1670 0.1680 0.1690 0.1560 0.1650 0.1620 0.1590];
% The tenors for the underlying swaps and the options on them
SwaptionExerciseDates = cellstr(['1M ';'2M ';'3M '; '6M ';'9M ';'1Y ';'18M';'2Y ';'3Y ';'4Y ';'5Y ';'6Y ';'7Y ';'8Y ';'9Y ';'10Y';'15Y';'20Y';'25Y';'30Y']);
SwaptionTenors = cellstr(['1Y ';
'2Y ';
'3Y ';
'4Y ';
'5Y ';
'6Y ';
'7Y ';
'8Y ';
'9Y ';
'10Y';
'15Y';
'20Y';
'25Y';
'30Y']);
testmat = zeros(length(SwaptionExerciseDates),1);
% Here I construct a matrix of exercise dates
for i = 1:length(SwaptionExerciseDates)
if SwaptionExerciseDates{i}(end)=='Y'
testmat(i) = addtodate(Settle,str2double(SwaptionExerciseDates{i}(1:end-1)),'year');
elseif SwaptionExerciseDates{i}(end)=='M'
testmat(i)=addtodate(Settle,str2double(SwaptionExerciseDates{i}(1:end-1)),'month');
end
end
EurExDates= testmat;
EurExDatesFull = repmat(testmat,1,length(SwaptionTenors));
testmat2 = zeros(length(SwaptionExerciseDates),length(SwaptionTenors));
% Here I construct a matix of maturity dates
for i = 1:size(EurExDatesFull,1)
for j = 1:size(EurExDatesFull,2)
if SwaptionTenors{j}(end)=='Y'
testmat2(i,j) = addtodate(EurExDatesFull(i,j),str2double(SwaptionTenors{j}(1:end-1)),'year');
elseif SwaptionTenors{j}(end)=='M'
testmat2(i,j)= addtodate(EurExDatesFull(i,j),str2double(SwaptionTenors{j}(1:end-1)),'month');
end
end
end
EurMatFull = testmat2;
% I construct an index of all the swaptions that I intend to use for calibration
relidx = find(EurMatFull <= InstrumentMaturity);
SwaptionBlackPrices = zeros(size(SwaptionBlackVol));
SwaptionStrike = zeros(size(SwaptionBlackVol));
% back out the swaption strikes and prices from the implied vol data.
for iSwaption=1:length(SwaptionExerciseDates)
for iTenor=1:length(SwaptionTenors)
[~,SwaptionStrike(iSwaption,iTenor)] = swapbyzero(RateSpec,[NaN 0],Settle, EurMatFull(iSwaption,iTenor),...
'StartDate',EurExDatesFull(iSwaption,iTenor),'LegReset',[1 2],'Basis',2);
SwaptionBlackPrices(iSwaption,iTenor) = swaptionbyblk(RateSpec,'call', SwaptionStrike(iSwaption,iTenor),Settle, ...
EurExDatesFull(iSwaption,iTenor), EurMatFull(iSwaption,iTenor),SwaptionBlackVol(iSwaption,iTenor));
end
end
TimeSpec = hwtimespec(Settle,daysadd(Settle,30*(1:370),6), 12);
% construct an index of some random collection of instruments
B = (214:224);
HW1Fobjfun4 = #(x) SwaptionBlackPrices(relidx(B)) - ...
swaptionbyhw(hwtree(hwvolspec(ValuationDate,testmat,x(2),testmat,x(1),'spline'), RateSpec, TimeSpec), 'call',SwaptionStrike(relidx(B)),EurExDatesFull(relidx(B)), 0,EurExDatesFull(relidx(B)), EurMatFull(relidx(B)),'Basis',2, 'SwapReset',12);
options = optimset('disp','iter','MaxFunEvals',1000,'TolFun',1e-5);
x0 = [.1 .01];
lb = [0 0];
ub = [1 1];
HW1Fparams = lsqnonlin(HW1Fobjfun4,x0,lb,ub,options)
In the results, when I just use instruments numbered 214:224, I am able to find a local minima: HW1Fparams(1) = 0.0801 , HW1Fparams(2) = 0.0002
however when I use instruments say 150:224 I get the following error:
"subscript indices must be real positive integers or logicals" error in cummswapcfbytrintree (line 23)
This is a Matlab created file. Does anyone know what's going wrong here? How do I go about diagnosing/debugging in the error is in some Matlab created file. The usual techniques of checking which variable or which index is being given a zero or double value are not at my disposal anymore. Thanks.

import data from a text file in MATLAB with multiple delimiters

I try to import data from a text file to MATLAB, which has the following structure:
** Porosity
**
*POR *ALL
0.1500 0.0900 2*0.1300 0.1400 4*0.1500 0.2200 2*0.1500 0.0500
0.0900 0.1400 5*0.1500 0.2300 0.2600 0.0800 0.1500 0.1500 0.2400 0.1700
[...]
The header has to be ignored obviously. Space is the delimiter, while * indicates that the same value occurs several times as indicated by the integer before the *.
Unfortunately, the number of entries per line varies. Ideally I want to store all values in one array like this:
por = [0.1500 0.0900 0.1300 0.1300 0.1400 0.1500 0.1500 0.1500 0.1500 0.1500 0.2200 0.1500 0.1500 ...]
Can this be solved with the textscan command somehow? The file is rather large with some hundred thousand values, so I need a quick solution ;) Help is greatly appreciated!
Straight forward way (I did not use Matlab for a long period of time, so it might be not the best solution)
fid = fopen('temp.txt');
data = textscan(fid, '%s', 'delimiter', ' ');
fclose(fid);
out = convert_cells(data);
And function
function out = convert_cells(cells)
out = [];
for i = 1 : size(cells{1})
tmp = strsplit(cells{1}{i}, '*');
num1 = str2double(tmp(1));
if size(tmp, 2) == 2 && ~isnan(num1)
num2 = str2double(tmp(2));
if ~isnan(num2)
out = [out repmat(num2, 1, num1)];
end;
elseif size(tmp, 2) == 1 && ~isnan(num1)
out(end + 1) = num1;
end;
end;
end

Extended Kalman Filter prediction Matlab

I'v implemented EKF (Extended Kalman Filter) in Matlab for Visual Tracking of Object's 3D trajectory, However, I'm giving it actual trajectory's position and velocity as in1 and in2 respectively. I'm facing wrong prediction after some points which is usually opposite to the actual trajectory. What I think, it may be some initial assumptions problem as I'v checked the equations many times but unable to find the bug. How to fix my initial assumptions or to avoid by any other guesses? Sample position and velocity are given.
Any suggestions for improving my approach are welcomed.
My Code:
function EKF(in1,in2)
ind=0; % indicator function. Used for unwrapping of tan
[H] = [];
[K] = [];
[Z] = [];
Q=[0 0 0 0 0 0;
0 0 0 0 0 0;
0 0 0 0 0 0;
0 0 0 0.01 0 0;
0 0 0 0 0.01 0;
0 0 0 0 0 0.01];% Covarience matrix of process noise
M=[0.001 0 0; 0 0.001 0; 0 0 0.001]; % Covarience matrix of measurment noise
A=[1 0 0 0.1 0 0;
0 1 0 0 0.1 0;
0 0 1 0 0 0.1;
0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 0 1]; % System Dynamics
in = cat(2,in1,in2);
X(:,1)=in(1,:)'; % Actual initial conditions
Z(:,1)=X(1:3,:);% initial observation
Xh(:,1)=X(:,1);%Assumed initial conditions
P(:,:,1)=[0.1 0 0 0 0 0;
0 0.1 0 0 0 0;
0 0 0.1 0 0 0;
0 0 0 0.1 0 0;
0 0 0 0 0.1 0;
0 0 0 0 0 0.1]; %inital value of covarience of estimation error
% plots
subplot(3,3,1)
xlabel('time')
ylabel('X')
title('X possition')
hold on
subplot(3,3,4)
xlabel('time')
ylabel('Y')
title('Y possition')
hold on
subplot(3,3,7)
xlabel('time')
ylabel('Z')
title('Z possition')
hold on
subplot(2,2,2)
xlabel('time')
title('Minimum MSE')
hold on
subplot(2,2,4)
plot3(0,0,0)
title('3-D trajectory ')
xlabel('X')
ylabel('Y')
zlabel('Z')
hold on
for n=1:100
%% PROCESS AND OBSERVATION PROCESS WITH GAUSSINA NOISE
X(:,n+1)=A*X(:,n)+[0;0;0;sqrt(Q(4,4))*randn(1);sqrt(Q(5,5))*randn(1);sqrt(Q(6,6))*randn(1)]; % State process % w generating process noise
Z(:,n+1)=[sqrt(X(1,n)^2+X(2,n)^2);arctang(X(2,n),X(1,n),ind);X(3,n)]+[sqrt(M(1,1))*randn(1);sqrt(M(1,1))*randn(1);sqrt(M(1,1))*randn(1)]; %generating & observation observation noise
%% prediction of next state
Xh(:,n+1)=A*Xh(:,n);% ESTIMATE
P(:,:,n+1)=A*P(:,:,n)*A'+Q;% PRIORY ERROR COVARIENCE
%% CORRECTION EQUTIONS
H(:,:,n+1)=[Xh(1,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)), Xh(2,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)),0,0,0,0; ...
-Xh(2,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)), Xh(1,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)),0,0,0,0; ...
0,0,1,0,0,0]; % Jacobian matrix
K(:,:,n+1)=P(:,:,n+1)*H(:,:,n+1)'*(M+H(:,:,n+1)*P(:,:,n+1)*H(:,:,n+1)')^(-1); % Kalman Gain
Inov=Z(:,n+1)-[sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2);arctang(Xh(2,n+1),Xh(1,n+1),ind);Xh(3,n+1)];% INNOVATION
Xh(:,n+1)=Xh(:,n+1)+ K(:,:,n+1)*Inov; %computes final estimate
P(:,:,n+1)=(eye(6)-K(:,:,n+1)*H(:,:,n+1))*P(:,:,n+1);% %computes covarience of estimation error
%% unwrapping the tan function
theta1=arctang(Xh(1,n+1),Xh(2,n+1),0);
theta=arctang(Xh(1,n),Xh(2,n),0);
if abs(theta1-theta)>=pi
if ind==1
ind=0;
else
ind=1;
end
end
%% Some Plots
subplot(3,3,1)
line([n,n+1],[X(1,n),X(1,n+1)])
hold on
drawnow
subplot(3,3,4)
line([n,n+1],[X(2,n),X(2,n+1)])
hold on
drawnow
subplot(3,3,7)
line([n,n+1],[X(3,n),X(3,n+1)])
hold on
drawnow
subplot(2,2,4)
line([Xh(1,n) Xh(1,n+1)],[Xh(2,n) Xh(2,n+1)],[Xh(3,n) Xh(3,n+1)],'Color','r')
hold on
drawnow
line([X(1,n) X(1,n+1)],[X(2,n) X(2,n+1)],[X(3,n) X(3,n+1)])
hold on
drawnow
subplot(2,2,2)
line([n,n+1],[(X(1,n)-Xh(1,n))^2,(X(1,n+1)-Xh(1,n+1))^2])
hold on
drawnow
line([n,n+1],[(X(2,n)-Xh(2,n))^2,(X(2,n+1)-Xh(2,n+1))^2],'Color','r')
hold on
drawnow
line([n,n+1],[(X(3,n)-Xh(3,n))^2,(X(3,n+1)-Xh(3,n+1))^2],'Color','c')
hold on
drawnow
legend('X-MSE','Y-MSE','Z-MSE')
subplot(3,3,1)
line([n,n+1],[Xh(1,n),Xh(1,n+1)],'Color','r')
hold on
drawnow
subplot(3,3,4)
line([n,n+1],[Xh(2,n),Xh(2,n+1)],'Color','r')
hold on
drawnow
subplot(3,3,7)
line([n,n+1],[Xh(3,n),Xh(3,n+1)],'Color','r')
hold on
drawnow
end
%% arctang
function [ARG]=arctang(A,B,ind)
if B<0 && A>0 % PLACING IN THE RIGHT QUADRANT
ARG=abs(atan(A/B))+pi/2;
elseif B<0 && A<0
ARG=abs(atan(A/B))+pi;
elseif B>0 && A<0
ARG=abs(atan(A/B))+3*pi/2;
else
ARG=atan(A/B);
end
if ind==-1 % UNWARPPING PART
ARG=ARG-2*pi;
else
if ind==1;
ARG=ARG+2*pi;
end
end
end
end
in1 - position of object
-21.8318 19.2251 -16.0000
-21.4604 18.9727 -15.8555
-21.0925 18.7208 -15.7102
-20.7281 18.4693 -15.5639
-20.3671 18.2182 -15.4169
-20.0093 17.9676 -15.2692
-19.6547 17.7173 -15.1208
-19.3031 17.4674 -14.9717
-18.9545 17.2178 -14.8221
-18.6088 16.9687 -14.6719
-18.2658 16.7198 -14.5213
-17.9255 16.4714 -14.3702
-17.5879 16.2232 -14.2187
-17.2528 15.9755 -14.0669
-16.9203 15.7281 -13.9147
-16.5901 15.4811 -13.7623
-16.2623 15.2345 -13.6097
-15.9368 14.9883 -13.4569
-15.6135 14.7425 -13.3040
-15.2925 14.4971 -13.1509
-14.9735 14.2522 -12.9978
-14.6567 14.0078 -12.8446
-14.3420 13.7638 -12.6914
-14.0292 13.5204 -12.5383
-13.7185 13.2776 -12.3852
-13.4097 13.0353 -12.2322
-13.1028 12.7936 -12.0793
-12.7979 12.5525 -11.9266
-12.4948 12.3122 -11.7741
-12.1935 12.0725 -11.6218
-11.8941 11.8336 -11.4697
-11.5965 11.5954 -11.3179
-11.3007 11.3581 -11.1663
-11.0067 11.1216 -11.0151
-10.7144 10.8860 -10.8642
-10.4240 10.6513 -10.7137
-10.1353 10.4176 -10.5635
-9.8483 10.1850 -10.4138
-9.5631 9.9533 -10.2644
-9.2797 9.7228 -10.1155
-8.9980 9.4934 -9.9671
-8.7181 9.2652 -9.8191
-8.4399 9.0382 -9.6717
-8.1636 8.8125 -9.5247
-7.8890 8.5882 -9.3783
-7.6162 8.3652 -9.2325
-7.3452 8.1436 -9.0872
-7.0760 7.9234 -8.9424
-6.8087 7.7048 -8.7983
-6.5432 7.4877 -8.6548
-6.2795 7.2723 -8.5119
-6.0178 7.0584 -8.3696
-5.7579 6.8463 -8.2280
-5.5000 6.6359 -8.0870
-5.2440 6.4273 -7.9467
-4.9899 6.2206 -7.8071
-4.7378 6.0157 -7.6681
-4.4878 5.8127 -7.5299
-4.2397 5.6118 -7.3923
-3.9938 5.4128 -7.2555
-3.7499 5.2159 -7.1194
-3.5080 5.0211 -6.9841
-3.2684 4.8285 -6.8495
-3.0308 4.6380 -6.7156
-2.7955 4.4498 -6.5825
-2.5624 4.2639 -6.4502
-2.3315 4.0803 -6.3187
-2.1028 3.8990 -6.1879
-1.8765 3.7202 -6.0579
-1.6525 3.5438 -5.9287
-1.4308 3.3699 -5.8004
-1.2115 3.1985 -5.6728
-0.9946 3.0297 -5.5460
-0.7801 2.8635 -5.4200
-0.5681 2.6999 -5.2949
-0.3586 2.5390 -5.1706
-0.1516 2.3807 -5.0471
0.0528 2.2253 -4.9245
0.2547 2.0725 -4.8027
0.4539 1.9226 -4.6817
0.6506 1.7755 -4.5616
0.8446 1.6313 -4.4423
1.0358 1.4899 -4.3239
1.2244 1.3514 -4.2063
1.4103 1.2159 -4.0897
1.5934 1.0833 -3.9738
1.7737 0.9538 -3.8589
1.9512 0.8272 -3.7448
2.1258 0.7036 -3.6315
2.2976 0.5831 -3.5192
2.4665 0.4657 -3.4077
2.6326 0.3513 -3.2971
2.7956 0.2400 -3.1874
2.9558 0.1318 -3.0786
3.1130 0.0268 -2.9707
3.2672 -0.0751 -2.8636
3.4184 -0.1739 -2.7575
3.5665 -0.2695 -2.6522
3.7117 -0.3620 -2.5479
3.8537 -0.4513 -2.4444
3.9927 -0.5374 -2.3419
4.1287 -0.6204 -2.2402
4.2615 -0.7002 -2.1395
4.3912 -0.7768 -2.0397
4.5178 -0.8503 -1.9408
4.6413 -0.9205 -1.8428
4.7616 -0.9876 -1.7457
4.8788 -1.0516 -1.6495
4.9928 -1.1124 -1.5542
5.1036 -1.1700 -1.4599
5.2113 -1.2246 -1.3665
5.3158 -1.2760 -1.2740
5.4171 -1.3242 -1.1825
5.5153 -1.3694 -1.0919
5.6102 -1.4115 -1.0022
5.7020 -1.4506 -0.9135
5.7906 -1.4866 -0.8257
5.8760 -1.5195 -0.7388
5.9583 -1.5495 -0.6529
6.0373 -1.5765 -0.5679
6.1132 -1.6005 -0.4839
6.1860 -1.6216 -0.4008
6.2555 -1.6398 -0.3187
6.3220 -1.6551 -0.2376
6.3853 -1.6675 -0.1574
6.4455 -1.6772 -0.0781
6.5026 -1.6840 0.0001
6.5565 -1.6882 0.0774
6.6074 -1.6896 0.1538
6.6553 -1.6883 0.2291
6.7000 -1.6844 0.3035
6.7418 -1.6779 0.3769
6.7805 -1.6688 0.4493
6.8163 -1.6572 0.5207
6.8491 -1.6431 0.5911
6.8789 -1.6266 0.6606
6.9058 -1.6077 0.7290
6.9298 -1.5865 0.7965
6.9510 -1.5630 0.8629
6.9693 -1.5373 0.9283
6.9847 -1.5093 0.9927
6.9974 -1.4792 1.0562
7.0074 -1.4470 1.1185
7.0146 -1.4128 1.1799
7.0191 -1.3766 1.2403
7.0210 -1.3385 1.2996
7.0203 -1.2985 1.3579
7.0170 -1.2567 1.4151
7.0111 -1.2132 1.4714
7.0027 -1.1679 1.5265
6.9919 -1.1210 1.5807
6.9786 -1.0726 1.6338
6.9629 -1.0226 1.6858
6.9449 -0.9712 1.7368
6.9246 -0.9184 1.7867
6.9020 -0.8642 1.8356
6.8773 -0.8088 1.8834
6.8503 -0.7522 1.9301
6.8212 -0.6945 1.9758
6.7901 -0.6357 2.0204
6.7569 -0.5759 2.0639
6.7218 -0.5152 2.1064
6.6847 -0.4536 2.1478
6.6458 -0.3912 2.1881
6.6050 -0.3280 2.2273
6.5625 -0.2643 2.2654
6.5183 -0.1999 2.3024
6.4723 -0.1350 2.3383
6.4248 -0.0696 2.3732
6.3757 -0.0039 2.4069
6.3251 0.0622 2.4396
6.2731 0.1285 2.4711
6.2197 0.1950 2.5016
6.1649 0.2616 2.5309
6.1089 0.3283 2.5592
6.0516 0.3949 2.5863
5.9932 0.4614 2.6124
5.9337 0.5278 2.6373
5.8731 0.5940 2.6611
5.8116 0.6598 2.6839
5.7491 0.7253 2.7055
5.6857 0.7904 2.7261
5.6216 0.8549 2.7455
5.5566 0.9189 2.7638
5.4910 0.9823 2.7811
5.4248 1.0450 2.7972
5.3579 1.1070 2.8122
5.2906 1.1681 2.8262
5.2228 1.2284 2.8391
5.1546 1.2877 2.8508
5.0860 1.3461 2.8615
5.0172 1.4034 2.8712
4.9481 1.4596 2.8797
4.8789 1.5146 2.8872
4.8096 1.5684 2.8936
4.7402 1.6210 2.8990
4.6708 1.6722 2.9033
4.6014 1.7221 2.9066
4.5322 1.7706 2.9088
4.4631 1.8176 2.9100
4.3943 1.8631 2.9102
4.3257 1.9070 2.9094
4.2574 1.9494 2.9076
4.1895 1.9901 2.9047
4.1221 2.0292 2.9009
4.0551 2.0666 2.8961
3.9886 2.1022 2.8904
3.9226 2.1361 2.8837
3.8573 2.1682 2.8761
3.7927 2.1985 2.8676
3.7287 2.2270 2.8581
3.6655 2.2536 2.8478
3.6030 2.2783 2.8365
3.5414 2.3011 2.8244
3.4806 2.3221 2.8115
3.4207 2.3411 2.7978
3.3618 2.3582 2.7832
3.3038 2.3734 2.7679
3.2467 2.3867 2.7517
3.1907 2.3980 2.7349
3.1357 2.4075 2.7173
3.0818 2.4150 2.6990
3.0290 2.4207 2.6800
2.9773 2.4245 2.6604
2.9267 2.4264 2.6401
2.8772 2.4266 2.6192
2.8289 2.4249 2.5978
2.7817 2.4215 2.5758
2.7358 2.4164 2.5533
2.6910 2.4095 2.5303
2.6474 2.4011 2.5068
2.6049 2.3910 2.4829
2.5637 2.3794 2.4587
2.5236 2.3664 2.4340
2.4846 2.3519 2.4091
2.4469 2.3361 2.3838
2.4102 2.3190 2.3584
2.3747 2.3008 2.3327
2.3403 2.2814 2.3068
2.3069 2.2610 2.2808
2.2746 2.2397 2.2548
2.2433 2.2175 2.2287
2.2130 2.1946 2.2026
2.1836 2.1711 2.1765
2.1551 2.1472 2.1506
2.1274 2.1228 2.1248
2.1006 2.0982 2.0992
2.0745 2.0735 2.0739
2.0491 2.0487 2.0489
2.0243 2.0242 2.0242
2.0000 2.0000 2.0000
in2 - Velocity of Object
0.3714 -0.2524 0.1445
0.3679 -0.2519 0.1454
0.3644 -0.2515 0.1462
0.3610 -0.2511 0.1470
0.3578 -0.2507 0.1477
0.3546 -0.2503 0.1484
0.3516 -0.2499 0.1491
0.3486 -0.2495 0.1496
0.3457 -0.2492 0.1502
0.3430 -0.2488 0.1506
0.3403 -0.2485 0.1511
0.3376 -0.2481 0.1515
0.3351 -0.2478 0.1518
0.3326 -0.2474 0.1521
0.3302 -0.2470 0.1524
0.3278 -0.2466 0.1526
0.3255 -0.2462 0.1528
0.3233 -0.2458 0.1530
0.3211 -0.2454 0.1531
0.3189 -0.2449 0.1531
0.3168 -0.2444 0.1532
0.3148 -0.2439 0.1532
0.3127 -0.2434 0.1531
0.3107 -0.2429 0.1531
0.3088 -0.2423 0.1530
0.3069 -0.2417 0.1529
0.3050 -0.2410 0.1527
0.3031 -0.2404 0.1525
0.3012 -0.2397 0.1523
0.2994 -0.2389 0.1521
0.2976 -0.2382 0.1518
0.2958 -0.2373 0.1515
0.2940 -0.2365 0.1512
0.2922 -0.2356 0.1509
0.2905 -0.2347 0.1505
0.2887 -0.2337 0.1502
0.2870 -0.2327 0.1498
0.2852 -0.2316 0.1493
0.2834 -0.2305 0.1489
0.2817 -0.2294 0.1484
0.2799 -0.2282 0.1480
0.2781 -0.2270 0.1475
0.2764 -0.2257 0.1469
0.2746 -0.2244 0.1464
0.2728 -0.2230 0.1459
0.2710 -0.2216 0.1453
0.2692 -0.2201 0.1447
0.2673 -0.2186 0.1441
0.2655 -0.2171 0.1435
0.2636 -0.2155 0.1429
0.2618 -0.2138 0.1423
0.2599 -0.2121 0.1416
0.2579 -0.2104 0.1410
0.2560 -0.2086 0.1403
0.2540 -0.2068 0.1396
0.2521 -0.2049 0.1389
0.2501 -0.2030 0.1382
0.2480 -0.2010 0.1375
0.2460 -0.1990 0.1368
0.2439 -0.1969 0.1361
0.2418 -0.1948 0.1353
0.2397 -0.1926 0.1346
0.2375 -0.1904 0.1339
0.2353 -0.1882 0.1331
0.2331 -0.1859 0.1323
0.2309 -0.1836 0.1315
0.2286 -0.1812 0.1308
0.2263 -0.1788 0.1300
0.2240 -0.1764 0.1292
0.2217 -0.1739 0.1284
0.2193 -0.1714 0.1276
0.2169 -0.1688 0.1268
0.2145 -0.1662 0.1260
0.2120 -0.1636 0.1251
0.2095 -0.1609 0.1243
0.2070 -0.1582 0.1235
0.2044 -0.1555 0.1226
0.2019 -0.1527 0.1218
0.1993 -0.1499 0.1210
0.1966 -0.1471 0.1201
0.1940 -0.1442 0.1193
0.1913 -0.1414 0.1184
0.1886 -0.1385 0.1176
0.1858 -0.1355 0.1167
0.1831 -0.1326 0.1158
0.1803 -0.1296 0.1150
0.1775 -0.1266 0.1141
0.1747 -0.1236 0.1132
0.1718 -0.1205 0.1123
0.1689 -0.1175 0.1115
0.1660 -0.1144 0.1106
0.1631 -0.1113 0.1097
0.1601 -0.1082 0.1088
0.1572 -0.1051 0.1079
0.1542 -0.1019 0.1070
0.1512 -0.0988 0.1061
0.1482 -0.0956 0.1053
0.1451 -0.0925 0.1044
0.1421 -0.0893 0.1035
0.1390 -0.0861 0.1025
0.1359 -0.0830 0.1016
0.1328 -0.0798 0.1007
0.1297 -0.0766 0.0998
0.1266 -0.0734 0.0989
0.1235 -0.0703 0.0980
0.1203 -0.0671 0.0971
0.1172 -0.0640 0.0962
0.1140 -0.0608 0.0952
0.1108 -0.0577 0.0943
0.1077 -0.0545 0.0934
0.1045 -0.0514 0.0925
0.1013 -0.0483 0.0915
0.0981 -0.0452 0.0906
0.0950 -0.0421 0.0897
0.0918 -0.0390 0.0887
0.0886 -0.0360 0.0878
0.0854 -0.0330 0.0869
0.0822 -0.0300 0.0859
0.0791 -0.0270 0.0850
0.0759 -0.0240 0.0840
0.0727 -0.0211 0.0831
0.0696 -0.0182 0.0821
0.0664 -0.0153 0.0812
0.0633 -0.0125 0.0802
0.0602 -0.0096 0.0792
0.0571 -0.0069 0.0783
0.0540 -0.0041 0.0773
0.0509 -0.0014 0.0763
0.0478 0.0013 0.0754
0.0448 0.0039 0.0744
0.0417 0.0065 0.0734
0.0387 0.0091 0.0724
0.0357 0.0116 0.0714
0.0328 0.0141 0.0704
0.0298 0.0165 0.0694
0.0269 0.0189 0.0684
0.0240 0.0212 0.0674
0.0211 0.0235 0.0664
0.0183 0.0258 0.0654
0.0155 0.0279 0.0644
0.0127 0.0301 0.0634
0.0099 0.0322 0.0624
0.0072 0.0342 0.0614
0.0045 0.0362 0.0603
0.0019 0.0381 0.0593
-0.0007 0.0400 0.0583
-0.0033 0.0418 0.0573
-0.0059 0.0436 0.0562
-0.0084 0.0453 0.0552
-0.0108 0.0469 0.0541
-0.0133 0.0485 0.0531
-0.0157 0.0500 0.0520
-0.0180 0.0514 0.0510
-0.0203 0.0528 0.0499
-0.0226 0.0541 0.0489
-0.0248 0.0554 0.0478
-0.0269 0.0566 0.0467
-0.0291 0.0577 0.0457
-0.0311 0.0588 0.0446
-0.0332 0.0598 0.0435
-0.0351 0.0607 0.0425
-0.0371 0.0616 0.0414
-0.0389 0.0624 0.0403
-0.0408 0.0631 0.0392
-0.0425 0.0638 0.0381
-0.0443 0.0644 0.0370
-0.0459 0.0649 0.0359
-0.0475 0.0654 0.0348
-0.0491 0.0657 0.0337
-0.0506 0.0661 0.0326
-0.0520 0.0663 0.0316
-0.0534 0.0665 0.0305
-0.0548 0.0666 0.0294
-0.0560 0.0667 0.0283
-0.0573 0.0666 0.0271
-0.0584 0.0665 0.0260
-0.0595 0.0664 0.0249
-0.0606 0.0661 0.0238
-0.0616 0.0659 0.0227
-0.0625 0.0655 0.0216
-0.0634 0.0651 0.0205
-0.0642 0.0646 0.0194
-0.0649 0.0640 0.0183
-0.0656 0.0634 0.0172
-0.0663 0.0627 0.0161
-0.0668 0.0620 0.0150
-0.0673 0.0611 0.0140
-0.0678 0.0603 0.0129
-0.0682 0.0593 0.0118
-0.0685 0.0583 0.0107
-0.0688 0.0573 0.0096
-0.0691 0.0562 0.0086
-0.0692 0.0550 0.0075
-0.0693 0.0538 0.0064
-0.0694 0.0526 0.0054
-0.0694 0.0512 0.0043
-0.0693 0.0499 0.0033
-0.0692 0.0485 0.0022
-0.0691 0.0470 0.0012
-0.0689 0.0455 0.0002
-0.0686 0.0440 -0.0008
-0.0683 0.0424 -0.0018
-0.0679 0.0407 -0.0028
-0.0675 0.0391 -0.0038
-0.0670 0.0374 -0.0048
-0.0665 0.0357 -0.0057
-0.0659 0.0339 -0.0067
-0.0653 0.0321 -0.0076
-0.0647 0.0303 -0.0085
-0.0640 0.0285 -0.0095
-0.0632 0.0266 -0.0103
-0.0624 0.0247 -0.0112
-0.0616 0.0228 -0.0121
-0.0608 0.0209 -0.0129
-0.0599 0.0190 -0.0138
-0.0590 0.0171 -0.0146
-0.0580 0.0152 -0.0154
-0.0570 0.0133 -0.0161
-0.0560 0.0114 -0.0169
-0.0550 0.0094 -0.0176
-0.0539 0.0076 -0.0183
-0.0528 0.0057 -0.0190
-0.0517 0.0038 -0.0196
-0.0506 0.0020 -0.0203
-0.0495 0.0001 -0.0209
-0.0483 -0.0017 -0.0214
-0.0471 -0.0034 -0.0220
-0.0460 -0.0051 -0.0225
-0.0448 -0.0068 -0.0230
-0.0436 -0.0085 -0.0235
-0.0424 -0.0100 -0.0239
-0.0413 -0.0116 -0.0243
-0.0401 -0.0131 -0.0246
-0.0389 -0.0145 -0.0250
-0.0378 -0.0158 -0.0252
-0.0366 -0.0171 -0.0255
-0.0355 -0.0183 -0.0257
-0.0344 -0.0194 -0.0259
-0.0334 -0.0204 -0.0260
-0.0323 -0.0213 -0.0261
-0.0313 -0.0222 -0.0261
-0.0303 -0.0229 -0.0261
-0.0294 -0.0235 -0.0260
-0.0285 -0.0240 -0.0259
-0.0276 -0.0244 -0.0258
-0.0268 -0.0246 -0.0256
-0.0261 -0.0247 -0.0253
-0.0254 -0.0247 -0.0250
-0.0248 -0.0245 -0.0247
-0.0243 -0.0242 -0.0242
1.0000 1.0000 1.0000