Differences

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

--- cs:localization:kalman:algorithm:start [2017/01/21 15:02]
Brian Moore [Section 4 - Simulation]
+++ cs:localization:kalman:algorithm:start [2017/01/21 19:33] (current)
Brian Moore [Section 5 - Matlab Code]
@@ Line 33: / Line 33: @@
 $$
 \hat{X}_{k'} = A\hat{X}_{k-1} + Bu_{k-1}, \qquad
-P_{k'} = AP_{k-1}A' + BQ^2B' \\
+P_{k'} = AP_{k-1}A' + Bdiag(Q^2)B' \\
 \hat{K} = P_{k'}C'/(CPC'+diag(\sigma_m^2)) \\
@@ Line 60: / Line 60: @@
 In the simulation, the vehicle on the 2D plane has thrusters to let it control orientation and forward motion.  The process noise in the system is relatively low, but the sensors are noisy and ping less frequently.  From a constant starting point and random orientation, the submarine attempts to reach its target, meandering less as it becomes more confident in its knowledge of the world.
-The Scaled Covariance axes is a heatmap of the covariance matrix $P$ scaled by the standard deviations $\sigma_x, \sigma_y, and \sigma_\theta$ of each of the sensors.  The derivative values $\dot{x}, \dot{y}, and \dot{\theta}$ are scaled by $dt^{-1}\sigma_x, dt{-1}\sigma_y, and dt{-1}\sigma_\theta$  Note how the variances along the diagonal mostly drop below unity - suggesting the submarine is //more confident// in it's state estimation $\hat{X}$ than any individual sensor reading. Note also that the variances are all lower when the sensors are reporting in more frequently.  Finally note how the variances are all lower when the submarine is moving quickly. Disagreements between expected and measured position become much more significant when projected into the future with a high velocity.
+The Scaled Covariance axes is a heatmap of the covariance matrix $P$ scaled by the standard deviations $\sigma_x, \sigma_y,$ and $\sigma_\theta$ of each of the sensors.  The derivative values $\dot{x}, \dot{y},$ and $\dot{\theta}$ are scaled by $\frac{\sigma_x}{dt}, \frac{\sigma_y}{dt},$ and $\frac{\sigma_\theta}{dt}$  Note how the variances along the diagonal mostly drop below unity - suggesting the submarine is //more confident// in it's state estimation $\hat{X}$ than any individual sensor reading. Note also that the variances are all lower when the sensors are reporting in more frequently.  Finally note how the variances are all lower when the submarine is moving quickly. Disagreements between expected and measured position become much more significant when projected into the future with a high velocity.
 With sensor reporting at every 3 seconds:
@@ 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