User Tools
Differences
This shows you the differences between two versions of the page.
| Both sides previous revision Previous revision Next revision | Previous revision | ||
|
cs:localization:kalman:algorithm:start [2017/01/21 19:27] 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 243: | Line 235: | ||
| == Kalman Update Loop == | == Kalman Update Loop == | ||
| <code matlab> | <code matlab> | ||
| - | %% Run Kalman Filter | + | %% Run Kalman Filter |
| %Estimate new state from last state estimate | %Estimate new state from last state estimate | ||
| X_est(:,k+1) = A*X_est(:,k) + B*(rr(X_est(3,k))*motor*u(:,k)); | X_est(:,k+1) = A*X_est(:,k) + B*(rr(X_est(3,k))*motor*u(:,k)); | ||
| Line 252: | Line 244: | ||
| %Sensors only check in every n seconds | %Sensors only check in every n seconds | ||
| if((mod(k*dt,0.5)) == 0) | if((mod(k*dt,0.5)) == 0) | ||
| - | %Calculate Kalman Gain | + | %Calculate Kalman Gain |
| - | K = (P(:,:,k+1)*C')/(C*P(:,:,k+1)*C' + diag(sigma.^2)); | + | K = (P(:,:,k+1)*C')/(C*P(:,:,k+1)*C' + diag(sigma.^2)); |
| - | + | ||
| - | %Resolve new estimate | + | %Resolve new estimate |
| - | X_est(:,k+1) = X_est(:,k+1)+K*(Y(:,k+1) - C*X_est(:,k+1)); | + | X_est(:,k+1) = X_est(:,k+1)+K*(Y(:,k+1) - C*X_est(:,k+1)); |
| - | %Resolve new covariance matrix for estimate | + | %Resolve new covariance matrix for estimate |
| - | P(:,:,k+1) = (eye(size(X_est,1))-K*C)*P(:,:,k+1); | + | P(:,:,k+1) = (eye(size(X_est,1))-K*C)*P(:,:,k+1); |
| end | end | ||
| | | ||
| Line 312: | Line 304: | ||
| end | end | ||
| | | ||
| - | end | + | end %ends for k=1:T/dt loop |
| </code> | </code> | ||
