12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added stdev on fx fy

release/4.3a0
cbeall3 2014-07-02 18:47:44 -04:00
parent f261a6ddbc
commit 584743e106
1 changed files with 29 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

@ -130,7 +130,12 @@ for i=1:20
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
result = initial; result = initial;
else end
if i == 2
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end
if i > 1
if i < 11 if i < 11
step = move_forward; step = move_forward;
else else
@ -138,7 +143,7 @@ for i=1:20
end end
initial.insert(i,result.at(i-1).compose(step)); initial.insert(i,result.at(i-1).compose(step));
% fg.add(BetweenFactorPose3(i-1,i, step, covariance)); fg.add(BetweenFactorPose3(i-1,i, step, covariance));
deltaT = 1; deltaT = 1;
logmap = Pose3.Logmap(step); logmap = Pose3.Logmap(step);
@ -199,8 +204,16 @@ for i=1:20
currentVelocityGlobal = isam.calculateEstimate(currentVelKey); currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey); currentBias = isam.calculateEstimate(currentBiasKey);
%% Compute some marginals
marginal = isam.marginalCovariance(calibrationKey);
marginal_fx(i)=sqrt(marginal(1,1));
marginal_fy(i)=sqrt(marginal(2,2));
end end
hold off; hold off;
clf; clf;
@ -241,13 +254,22 @@ for i=1:20
fys(i) = fy; fys(i) = fy;
subplot(3,1,3); subplot(3,1,3);
hold on; hold on;
plot(1:20,repmat(K.fx,1,20),'b--'); p(1) = plot(1:20,repmat(K.fx,1,20),'r--');
plot(1:i,fxs,'b'); p(2) = plot(1:i,fxs,'r','LineWidth',2);
p(3) = plot(1:20,repmat(K.fy,1,20),'g--');
p(4) = plot(1:i,fys,'g','LineWidth',2);
plot(1:20,repmat(K.fy,1,20),'g--'); if i > 1
plot(1:i,fys,'g'); plot(2:i,fxs(2:i) + marginal_fx(2:i),'r-.');
plot(2:i,fxs(2:i) - marginal_fx(2:i),'r-.');
plot(2:i,fys(2:i) + marginal_fy(2:i),'g-.');
plot(2:i,fys(2:i) - marginal_fy(2:i),'g-.');
end
legend('f_x', 'f_x''', 'f_y', 'f_y''', 'Location', 'SouthWest');
legend(p, 'f_x', 'f_x''', 'f_y', 'f_y''', 'Location', 'SouthWest');
if(write_video) if(write_video)