This shows you the differences between two versions of the page.
Both sides previous revision Previous revision | |||
cs:localization:kalman:algorithm:start [2017/01/21 19:29] Brian Moore |
cs:localization:kalman:algorithm:start [2017/01/21 19:33] (current) Brian Moore [Section 5 - Matlab Code] |
||
---|---|---|---|
Line 153: | Line 153: | ||
%% Initialize graphics | %% Initialize graphics | ||
close all | close all | ||
- | % f2 = figure(2) | ||
- | % covP = imagesc(sqrt(P(:,:,1))); | ||
- | % covT = title(['Time = ',num2str(dt),'sec']) | ||
- | % colorbar | ||
- | |||
f1 = figure(1); | f1 = figure(1); | ||
- | %fT = title(['Time = ',num2str(dt),'sec']); | ||
set(f1, 'Position', [0 0 1920/2 1080/2]); | set(f1, 'Position', [0 0 1920/2 1080/2]); | ||
+ | |||
sp1 = subplot(1,2,1); | sp1 = subplot(1,2,1); | ||
hold on | hold on | ||
Line 175: | Line 170: | ||
utext = text(-1.5+X(1,1),3.5+X(2,1),['L = ',num2str(u(1,1)),'N R = ',num2str(u(1,1)),'N']); %disp thruster values | utext = text(-1.5+X(1,1),3.5+X(2,1),['L = ',num2str(u(1,1)),'N R = ',num2str(u(1,1)),'N']); %disp thruster values | ||
+ | |||
hold off | hold off | ||
xlim(2*[-1 1]), ylim(3*[-1 1]) | xlim(2*[-1 1]), ylim(3*[-1 1]) | ||
Line 216: | Line 212: | ||
| | ||
%how to fire thrusters for desired translation | %how to fire thrusters for desired translation | ||
- | % u_t = inv((rr(X_est(5,k))*B)'*(rr(X_est(5,k))*B))*(rr(X_est(5,k))*B)' * [f_goal(1);0]; | ||
- | % %for desired rotation | ||
- | % u_r = inv(rr(X_est(5,k))*B) * [0; f_goal(2)]; | ||
u_t = f_goal(1) * [1;1]; | u_t = f_goal(1) * [1;1]; | ||
u_r = f_goal(2) * [1;-1]; | u_r = f_goal(2) * [1;-1]; | ||
- | + | %Combine thrusts to acomplish goals - limit each goal to 50% thrust to truncate demand. | |
- | u(:,k) = sign(u_t).*min(abs(u_t),thrust/2) + sign(u_r).*min(abs(u_r),thrust/2); | + | u(:,k) = sign(u_t).*min(abs(u_t),thrust/2) + sign(u_r).*min(abs(u_r),thrust/2); |
</code> | </code> | ||
Line 229: | Line 222: | ||
%% Model Submarine/thrusters (Reality normally does this job) | %% Model Submarine/thrusters (Reality normally does this job) | ||
%calculate new state from last state | %calculate new state from last state | ||
- | %X(:,k+1) = A*X(:,k) + rr(X(3,k))*B*(u(:,k)) + q*Q*randn(3,1); | ||
X(:,k+1) = A*X(:,k) + B*(rr(X(3,k))*motor*u(:,k) + Q*randn(3,1)); | X(:,k+1) = A*X(:,k) + B*(rr(X(3,k))*motor*u(:,k) + Q*randn(3,1)); | ||
| | ||
Line 312: | Line 304: | ||
end | end | ||
| | ||
- | end | + | end %ends for k=1:T/dt loop |
</code> | </code> | ||