Differences

This shows you the differences between two versions of the page.

--- cs:localization:kalman:algorithm:start [2017/01/21 16:26]
Brian Moore [Section 2 - Kalman Filter Algorithm]
+++ cs:localization:kalman:algorithm:start [2017/01/21 19:33] (current)
Brian Moore [Section 5 - Matlab Code]
@@ Line 70: / Line 70: @@
 {{ cs:localization:kalman:kalman_sim_halfs.gif }}
+===== Section 5 - Matlab Code =====
+Unfortunately the code looks more complicated than what's above, because I through orientation into the mix, which adds a bit of complexity, and a control system necessary to make the little figure behave appropriately.  However, the parameters set should all look //very// familiar, as should the Kalman Update Process.  The Modeling code is normally handled by... well... reality. And the Control System producing the inputs from your sensor measurement estimates is some other poor shmuck.
+=== Model / Sensor Parameters ===
+Here we initialize all the matrices we'll need for our model. $A$ Transition Matrix, $B$ Input Matrix, $X$ State Vector, $u$ input vector, and Process noise $Q$, as well as general parameters like our time step $dt$ and physical parameters of our vehicle like mass and rotational inertia.  The Emission Matrix $C$ is also generated here, with corresponding sensor noises $\sigma$.
+<code matlab>
+%% Model parameters
+clear all
+close all
+dt = 0.1;       %sec (time step size)
+T = 200;        %sec (simulation run time)
+mass = 10;      %kg
+rI = 1;  %rotational inertia in Nms^2/degrees
+thrust = 10;     %Newtons of force per thruster
+t_rad = 0.2;    %meters from center thrusters mounted
+goal = [10;14];     %meters x,y goal coordinates
+cP = diag([1,1]);         %proportion control parameter dist,angle
+cD = diag([.05,1]);        %derivative control parameter
+%cI = 0;         %no buoyancy or anything to cause steady-state errors, so no intergral term
+err = zeros(2,T/dt);    %distance and angle error
+%initialize state
+X = zeros(6,T/dt);         % [x y theta dx dy dtheta ]'
+%create dynamics model - .98^10 drag / sec
+A = [1 0 0 .99*dt 0 0;...
+1 0 0 .99*dt 0;...
+0 1 0 0 .99*dt;...
+0 0 .98 0 0;...
+0 0 0 .98 0;...
+0 0 0 0 .98 ;];
+%create input dynamics      %for x, y, and theta forces/torques
+B = [dt^2/2/mass 0 0;...
+dt^2/2/mass 0;...
+0 dt^2/2/rI;...
+         dt/mass 0 0;...
+dt/mass 0;...
+0 dt/rI;];
+%vector of inputs - Left and Right thruster
+u =  zeros(2,T/dt);        %[left; right] thrusters
+%translates motor inputs to x,y,theta force/torque inputs = motor*u
+motor = [1 1; 0 0; -1 1];
+%Helper function - rotate thruster force from intrinsic to global coordinates
+rr = @(theta)[cosd(theta) -sind(theta) 0; sind(theta) cosd(theta) 0; 0 0 1];
+%Force/Torque = (rr(theta)*motor*u)
+%Process noise
+Q = thrust/10;  %std = +/-10% of max thrust
+%% Sensors
+% position sensor for x,y and oreintation sensor for yaw
+C = [1 0 0 0 0 0;...
+1 0 0 0 0;...
+0 1 0 0 0;];
+%Sensor noise
+sigma = [1.2; 1.2; 4];  %std x,y,theta - meters, meters, degrees
+%% Initialize Model Values:
+%Original Estimate
+X_est(1:6,1:T/dt) = 0;   %assume all states zero on start-up
+%Covariance Matrix (confidence of state estimation
+P(:,:,1) = 9999*eye(6);    %covariance matrix of state estimation
+%Initialize Model Values:
+X(3,1) = 100;   %random initial orientation
+</code>
+=== Initializing Graphic Objects ==
+This was just necessary to produce those nice .gif files. Ignore this unless you have some weird interest in matlab graphing syntax.
+<code matlab>
+%% Initialize graphics
+close all
+f1 = figure(1);
+set(f1, 'Position', [0 0 1920/2 1080/2]);
+sp1 = subplot(1,2,1);
+hold on
+tT = title(['Position at Time = ',num2str(dt),'sec']);
+gT = plot(goal(1),goal(2),'k*','MarkerSize',50);
+pT = scatter(X(1,1),X(2,1),'MarkerEdgeColor','k','MarkerEdgeAlpha',.4);
+vT = line([X(1,1),X(1,1)+X(4,1)/dt],[X(2,1),X(2,1)+X(5,1)/dt],'Color','g');
+oT = line([X(1,1),X(1,1)+cosd(X(3,1))/2],[X(2,1),X(2,1)+sind(X(3,1))/2],'Color','r');
+pE = scatter(X_est(1,1),X_est(2,1),'+','MarkerEdgeColor','b','MarkerEdgeAlpha',.4);
+vE = line([X_est(1,1),X_est(1,1)+X_est(4,1)/dt],[X_est(2,1),X_est(2,1)+X_est(5,1)/dt],'Color','y');
+oE = line([X_est(1,1),X_est(1,1)+cosd(X_est(3,1))/2],[X_est(2,1),X_est(2,1)+sind(X_est(3,1))/2],'Color','m');
+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
+xlim(2*[-1 1]), ylim(3*[-1 1])
+xlabel('X pos [m]'), ylabel('Y pos [m]')
+sp2 = subplot(1,2,2);
+covP = imagesc(sqrt([P(:,:,1);4*ones(1,6)]));
+covT = title(['Scale Covariance']);
+xlim(sp2,[.5 6.5]), ylim(sp2,[.5 6.5])
+xlabel('X [m] Y [m] \theta [deg] dX [m] dY [m] d\theta [m] '),
+ylabel('X [m] Y [m] \theta [deg] dX [m] dY [m] d\theta [m] '),
+colorbar
+frame(1) = getframe(f1);
+tic;
+</code>
+=== Model / Kalman Update Loop ===
+Here's the meat of what you're looking for - third codeblock down, the Kalman Update section
+<code matlab>
+%% Model / Filter update loop
+for k = 1:T/dt
+</code>
+== Control System Update ==
+<code matlab>
+%% Control System
+    %calculate error
+    err(1,k+1) = sqrt(sum((goal-X_est([1,2],k)).^2));
+    err(2,k+1) = atan2d(goal(2)-X_est(2,k),goal(1)-X_est(1,k)) - X_est(3,k);
+    err(2,k+1) = -err(2,k+1);   %correct for handed-ness
+    while(abs(err(2,k+1))>180)
+        err(2,k+1) = err(2,k+1) - sign(err(2,k+1))*360;
+    end
+    %calculate desired forces based on error
+    f_goal = cP*err(:,k+1) + cD*(err(:,k+1)-err(:,k))./dt;
+    f_goal(1) = cosd(err(2,k))*f_goal(1);   %reduce goal for wrong-facing direction
+    f_goal = f_goal.*dt;
+    %how to fire thrusters for desired translation
+    u_t = f_goal(1) * [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);
+</code>
+== Model Update ==
+<code matlab>
+    %% Model Submarine/thrusters (Reality normally does this job)
+    %calculate new state from last state
+    X(:,k+1) = A*X(:,k) + B*(rr(X(3,k))*motor*u(:,k) + Q*randn(3,1));
+    %correct rollover
+    while(abs(X(3,k+1))>180)
+        X(3,k+1) = X(3,k+1) - sign(X(3,k+1))*360;
+    end
+    %calculate new sensor measurements from new state
+    Y(:,k+1) = C*X(:,k+1) + sigma.*randn(3,1);
+</code>
+== Kalman Update Loop ==
+<code matlab>
+    %% Run Kalman Filter
+    %Estimate new state from last state estimate
+    X_est(:,k+1) = A*X_est(:,k) + B*(rr(X_est(3,k))*motor*u(:,k));
+    %Estimate confidence in new estimate - model process noise of inputs
+    % through input matrix
+    P(:,:,k+1) = A*P(:,:,k)*A' + B*diag([Q Q Q].^2)*B';
+    %Sensors only check in every n seconds
+    if((mod(k*dt,0.5)) == 0)
+        %Calculate Kalman Gain
+        K = (P(:,:,k+1)*C')/(C*P(:,:,k+1)*C' + diag(sigma.^2));
+        %Resolve new estimate
+        X_est(:,k+1) = X_est(:,k+1)+K*(Y(:,k+1) - C*X_est(:,k+1));
+        %Resolve new covariance matrix for estimate
+        P(:,:,k+1) = (eye(size(X_est,1))-K*C)*P(:,:,k+1);
+    end
+    %Correct rollover
+    while(abs(X_est(5,k+1))>180)
+        X_est(3,k+1) = X_est(3,k+1) - sign(X_est(3,k+1))*360;
+    end
+</code>
+== Graphic Update ==
+<code matlab>
+ %% Graphic update
+    %figure(1)
+    tT.String = ['Position at Time = ',num2str(k*dt+dt),'sec'];
+    pT.XData = X(1,1:k+1);
+    pT.YData = X(2,1:k+1);
+    vT.XData = [X(1,k+1),X(1,k+1)+X(4,k+1)/dt];
+    vT.YData = [X(2,k+1),X(2,k+1)+X(5,k+1)/dt];
+    oT.XData = [X(1,k+1),X(1,k+1)+cosd(X(3,k+1))/2];
+    oT.YData = [X(2,k+1),X(2,k+1)+sind(X(3,k+1))/2];
+    pE.XData = X_est(1,1:k+1);
+    pE.YData = X_est(2,1:k+1);
+    vE.XData = [X_est(1,k+1),X_est(1,k+1)+X_est(4,k+1)/dt];
+    vE.YData = [X_est(2,k+1),X_est(2,k+1)+X_est(5,k+1)/dt];
+    oE.XData = [X_est(1,k+1),X_est(1,k+1)+cosd(X_est(3,k+1))/2];
+    oE.YData = [X_est(2,k+1),X_est(2,k+1)+sind(X_est(3,k+1))/2];
+    utext.Position = [-1.5 3.5]+[X(1,k+1),X(2,k+1)];
+    utext.String = ['L = ',num2str(u(1,k)),'N R = ',num2str(u(2,k)),'N'];
+    xlim(sp1,X(1,k+1)+2*[-1 1]), ylim(sp1,X(2,k+1)+3*[-1, 1])
+    covP.CData = sqrt([diag(1./[sigma; dt*sigma])*P(:,:,k+1)*diag(1./[sigma; dt*sigma]);4*ones(1,6)]);
+    pause(dt/2-toc)
+    tic;
+    frame(ceil(k/2)+1) = getframe(f1);
+</code>
+That's the entirety of the update loop. It will stop when Time ''T/dt'' runs out or it hits an arbitrary stop condition
+<code matlab>
+ %% Stop condition - close to target
+    if(sum(abs(goal-X_est(1:2,k+1))) < .3)
+        break;
+    end
+end  %ends for k=1:T/dt loop
+</code>
+=== Graphics cleanup ===
+Why are you reading this?  It's uninteresting.
+<code matlab>
+%% clean up chart, close out and create gif
+delete(utext);
+for n=1:10
+frame(ceil(k/2)+1+n) = getframe(f1);
+end
+xlim(sp1,[-5 20]), ylim(sp1,[-7.5 30])
+for n=1:10
+frame(ceil(k/2)+11+n) = getframe(f1);
+end
+movie2gif(frame, 'kalman_sim_halfs.gif', 'DelayTime', dt,'Loopcount',inf)
+%%%End Program%%%
+</code>

User Tools

Differences

Page Tools

Palouse RoboSub Technical Documentation

Site Tools