User Tools
Differences
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> | ||
