Understanding the UKF SLAM in Matlab

In my previous post, I worked through a 2-D EKF SLAM (Simultaneous Localization and Mapping) homework assignment for the online course by Prof. Cyrill Stachniss.  In this blog entry, I get to solve the same problem using the UKF (Unscented Kalman Filter).  Being the same non-linear problem, the state formation and the equations of forward process evolution and observation are the same as in the EKF SLAM case.  It reading the exposition below, it might be helpful to pull up my previous post next to this post.

Prediction step

In EKF, I simply fed the current state estimate 𝛍^-[k] (and the external input u[k]) to the nonlinear function g(u[k]; 𝛍⁺[k-1]) to predict the next state:

𝛍^-[k] = g(u[k]; 𝛍⁺[k-1])

But in UKF, I have to go through the UT (Unscented Transform): generate an ensemble (called sigma points 𝛘⁺[k-1] in section 3.4.1 of the Probabilistic Robotics book) of 𝛍⁺[k-1], and feed all those points to g().  Then 𝛍^-[k] is the mean and the covariance of the transformed ensemble are the  new 𝛍^-[k] and 𝚺_𝛍𝛍^-[k].

Sigma points 𝛘

The ensemble 𝛘⁺[k-1] should be centered around 𝛍⁺[k-1], with enough "spread"--based on 𝚺_𝛍𝛍⁺[k-1].  𝛘⁺₀[k-1] is the current estimate 𝛍⁺[k-1].  Subsequent points are some + and - direction along each of dimension of the uncertainty.  I visualize this as the pair of points roughly 1~2 standard deviation on either side of 𝛍⁺[k-1].  A pair of points for each of the state dimension n produces 1 + 2n sample points, as you can see in the left ellipsoid below, which is rotated around the principle (x-y) axis because of non-zero cross-covariance term in 𝚺_𝛍𝛍⁺[k-1].

Unscented transform maps the sigma points through g()

Along with 𝛘⁺[k-1], we have to generate weights w_m and w_c to use when calculating the mean and the covariance of the transformed points, respectively.  In the book, w_m and w_c are the same except for w_c0 = w_m0 + (1 - 𝛂² + 𝛃).  Although the book presented only 1 sigma point generating and weighting algorithm, there are apparently many variations listed, for example, in Grewall and Andrews.  The only thing that did not understand is the extra (1 - 𝛂² + 𝛃) term on w_c0, because the weights are supposed to sum to 1 (normalized); I checked that w_m sums to 1, which means that w_c does not sum to 1.  Until I can tackle down the Ph.Ds in my team for a satisfying answer (don't hold you breadth), here is the Matlab code to generate the sigma points:

function [X, wm, wc] = compute_sigma_points(mu, sigma)

% Computes the 2n+1 sigma points according to the unscented transform,

% where n is the dimensionality of the mean vector mu.

% The sigma points should form the columns of sigma_points,

% i.e. sigma_points is an nx2n+1 matrix.

n = length(mu); % state dimension

global alpha kappa beta;

lambda = max(alpha^2 * (n + kappa) - n, 1);

scale = lambda + n;

[U, D, V] = svd(sigma); %[V,D] = eig(sigma);

D = sqrt(D);

D = max(D, 0.05 * eye(n)); % lower bound explained below

sigmasqr = ...sqrt(n+lambda) * Seems to be too big!

           ... sqrtm(sigma); %chol(sigma);

           U * D * V'; %V * D / V;

mureplicated = repmat(mu, 1, n);

X = [mu, mureplicated + sigmasqr, mureplicated - sigmasqr];

% Computing the weights for recovering the mean and variance.

% Note: weights are column vectors, to allow vector multiply w/ samples

wm = [lambda/scale; repmat(1/(2*scale), 2*n, 1)];

wc = wm;

wc(1) = wc(1) + 1 - alpha^2 + beta;

end

Prediction in SLAM

Unless some of the states are completely decoupled (the covariance between them is zero), one should feed the full 𝛍⁺[k-1] and 𝚺_𝛍𝛍⁺[k-1] to the compute_sigma_points.  But in SLAM, the landmarks do not move (one of the fundamental assumptions), so there is no need to predict the landmark positions, so I can just constrain the prediction to the robot pose.  In this 2-D planar robot, the I can use Matlab's vector operator to transform all 𝛘⁺[k-1] in just a few lines of code.

[X, wm, wc] = compute_sigma_points(mu(1:3), sigma(1:3,1:3));

theta = X(3, :);

ang = theta + rot1 + rot2;

pose_sample = [X(1, :) + trans * cos(theta + rot1);

               X(2, :) + trans * sin(theta + rot1);

               cos(ang); % expand out x,y separately

               sin(ang)];

% Be careful when computing the robot's orientation (sum up the sines and

% cosines and recover the 'average' angle via atan2)

mu(1:2) = pose_sample(1:2,:) * wm;

mu(3) = atan2(pose_sample(4,:) * wm, pose_sample(3,:) * wm);

I can see why w_m being normalized is important for keeping 𝛍^-[k] unbiased.  Prediction of the covariance is also concise in Matlab:

dSigma = X - mu(1:3) * ones(1,length(wm)); % subtract the mean

dSigma(3,:) = wrapToPi(dSigma(3,:)); % normalize angles again

sigma(1:3,1:3) = dSigma * diag(wc) * dSigma' ...

    + diag([0.01 0.01 deg2rad(0.5)]); % robot move uncertainty

Negative feedback loop between 𝚺_mm and sigma point spread

I first tried using the matrix square root or Cholesky decomposition to generate the pairs of points away from the mean, but I ran into a problem: there is a "negative" feedback between "uncertainty" (can be seen with the norm of the covariance matrix) and the sigma point spread.  Intuitively, if the sigma point spread scales linearly with the size of the hyper-ellipsoid, decreasing uncertainly leads to smaller spread in the sigma points, which (unless the nonlinear system equation is grossly non-linear or even singular) in turn leads to smaller covariance matrix norm; because 𝚺_mm does not grow during the prediction step like 𝚺_xx (see my earlier comment about the landmark stationarity assumption in SLAM), this leads to 𝚺_mm asymptotically decaying to 0. This contrasts sharply with the uncertainty of the landmark being lower bounded by the robot pose in EKF (they are completely correlated).  I by lower bounding the diagonal entries of the SVD(𝚺_𝛍𝛍), I tried to keep the landmark uncertainty disappearing to 0. I am not sure yet if this is the best way to deal with the problem, but 𝚺_mm --> 0 is a numerical problem at minimum because 𝚺_𝛍𝛍 becomes ill-conditioned.  Because I always think about "how am I going to do this on a 32-bit or 16-bit MCU", I am particularly alarmed about this inconvenient discovery that is usually not mentioned in the Kalman Filter fundamental books.  Even with this heuristics, the landmark position uncertainty has reduced dramatically compared to EKF.

Robot pose (magenta) and landmark position (cyan) estimate at the end of 331 time steps.  Note that the estimates have drifted less than in EKF SLAM.

Correction step

The unscented transform is used again during the correction step--this time using the a-priori mean 𝛍^-[k] and covariance 𝚺_𝛍𝛍^-[k] from the prediction step, and using all of them instead of just the robot pose portion, as in the SLAM prediction step.  To correct the states from observation, I need 2 things: the Kalman gain K[k] and the innovations from the landmark observations.  But at a given time step k, the observation consists of old landmarks (from which innovation can be formed) and new landmarks.  So I have to form K_p[k] and an ensemble of innovations (z_p[k] - hp(𝛘^-[k])) using just previously observed landmarks at time step k (subscript "p" for previously known).  Note that I cannot reuse the ensemble generated during prediction because the predicted state 𝛍^-[k] is different than 𝛍⁺[k-1].  Forming the innovations from z_p[k] and 𝛘^-[k] is relatively straight-forward, but K_p[k] involves more steps.  For clarity, let the ensemble of expected observations be

Z_p[k] = h_p(𝛘^-[k])

z_p^avg[k] = Z_p[k] w_m

Then loosely speaking, K_p[k] is the cross-correlation of 𝛘^-[k] with Z_p[k], divided by the covariance of Z[k]:

S_p[k] = (Z_p[k] - z_p^avg[k]) diag(w_c) (Z_p[k] - z_p^avg[k])^T + Q_p[k]

K_p[k] = (𝛘^-[k] - 𝛍^-[k]) diag(w_c) (Z_p[k] - z_p^avg[k])^T / S_p[k]

The uncertainty of measurement Q_p[k] works to de-weight the innovation for the correction, just like in EKF.  The state and covariance correction are then:

𝛍⁺[k] = 𝛍^-[k] + K_p[k] ( z[k] - z_p^avg[k] )

𝚺_𝛍𝛍⁺[k] = 𝚺_𝛍𝛍^-[k] - K_p[k] S_p[k] K_p^T[k]

Here is the Matlab implementation of the simple idea given above:

lId = z(:,1);

oz = z(ismember(lId, map), :);

if ~isempty(oz) % Have known landmark => can generate innovation

    [X, wm, wc] = compute_sigma_points(mu, sigma);

    nX = size(X,2); % number of sigma points

    % Reorder obs according to map, which is also the order in mu (state)

    [~,lIdx] = ismember(oz(:,1), map);

    nz = size(oz, 1);

    % line 7 on slide 32: Z = h(X)

    % Generate distribution of expected observation from sigma_point of

    % robot poses AND landmark positions. 

    Dx = X(2*lIdx+2,:) - ones(nz,1) * X(1,:); % nZ x nX

    Dy = X(2*lIdx+3,:) - ones(nz,1) * X(2,:); % nZ x nX

    

    Range = sqrt(Dx.^2 + Dy.^2); % predicted range and bearing distribution

    Bearing = atan2(Dy, Dx) - ones(nz,1) * X(3,:);

    % To average the angle, must go through unit circle

    Cos = cos(Bearing) * wm; Sin = sin(Bearing) * wm;

    Range_avg = Range * wm;

    % Rearrange according to the mu order

    Z_avg = zeros(2*nz, 1);

    Z_avg(1:2:end) = Range_avg;

    Z_avg(2:2:end) = atan2(Sin, Cos);

    q = zeros(2*nz,1);

    q(1:2:end) = Range_avg * 0.01;

    q(2:2:end) = deg2rad(1) * ones(nz,1);

    q = q .^ 2;

    

    % UKF correction algo step 9

    dZ = zeros(2*nz, nX);

    dZ(1:2:end, :) = Range;

    dZ(2:2:end, :) = Bearing;

    % innovation covariance matrix S, line 9 on slide 32

    dZ = dZ - Z_avg * ones(1,nX); % Subtract the mean of sample

    % Normalize the angles

    dZ(2:2:end) = wrapToPi(dZ(2:2:end));

    S = dZ * diag(wc) * dZ' + diag(q);

    % Compute Sigma_x_z, the Cross covariance, line 10 on slide 32

    % (which is equivalent to sigma times the Jacobian H transposed in EKF).    

    dSigma = X - mu * ones(1, nX); % subtract the mean

    dSigma(3,:) = wrapToPi(dSigma(3,:)); % normalize angles again

    Sigma_xz = dSigma * diag(wc) * dZ';

    % Compute the Kalman gain, line 11 on slide 32

    K = Sigma_xz / S;

    % Current observation

    %lId = oz(:,1);

    range = oz(:,2); bearing = oz(:,3);

    % Update mu and sigma, line 12 + 13 on slide 32

    innoZ = zeros(2*nz,1);

    innoZ(1:2:end) = range; % Form the innovation

    innoZ(2:2:end) = bearing;

    innoZ = innoZ - Z_avg;

    innoZ(2:2:end) = wrapToPi(innoZ(2:2:end));

    

    mu = mu + K * innoZ;

    mu(3) = wrapToPi(mu(3));

    sigma = sigma - K * S * K';

end

Accepting new landmarks

Previously unobserved landmarks are no use for correction, but still have to be accepted into an expanding state.  I have not yet figured out how to calculate the cross-correlation of the newly observed landmark with existing landmark or even the robot, but here's my code used to generate the above result:

z = z(~ismember(z(:,1), map), :); % Deal with only the new landmarks

nz = size(z,1);

for i=1:nz % For newly discovered landmarks in this timestep, sample around

    nMu = length(mu);

    lId = z(i,1); range = z(i,2); bearing = z(i,3); % raw measurement

    [X, wm, wc] = compute_sigma_points([range; bearing] ...

        , diag([0.01*range deg2rad(1)] .^ 2));

    % Transform range/bearing to Cartesian

    Range = X(1,:);

    Bearing = X(2,:);

    Dx = Range .* cos(Bearing + mu(3));

    Dy = Range .* sin(Bearing + mu(3));

    dx = Dx * wm;% Expected landmark position relative to robot position

    dy = Dy * wm;% = weighted average

    mu(nMu + (1:2)) = [mu(1) + dx; mu(2) + dy];

    dSigma = [Dx - dx; Dy - dy];

    % Stick the new weighted variance at the lower right corner of sigma

    sigma(nMu+(1:2),nMu+(1:2)) = dSigma * diag(wc) * dSigma';

    map(end+1) = lId; %add landmark to the map

end

Understanding the EKF SLAM

In April of this year, I attended the AR (augmented reality) workshop at Stanford, and learned the current issues and direction of the AR community.  I was curious whether I can just use VI (visual-inertial) SLAM (simultaneous localization and mapping) for the AR video game I want to write, and got confirmation that this would be possible after talking to Renato F. Salas-Moreno.  Since I work with inertial sensors in my current job, VI SLAM is particularly interesting.  While the VI SLAM on a single device is amply demonstrated by the slew of cutting edge products like Google Tango and Microsoft HoloLens, the interactive AR video game I have in mind requires merging of the map produced from multiple devices in the same room.  I learned from Marc Pollefeys that HoloLens solves the problem by using the map of a "master" device.  I got the feeling that this is not a completely solved problem, but since I am new to the whole SLAM problem, I am going to work through the EKF (extended Kalman filter) example from the online SLAM course, which uses the notation in the Probabilistic Robotics book.  In the equations to follow, I am forced to use the underline as the vector, instead of the usual overbar notation, because of the poor formatting capability of Blogger editor.

State formation

In SLAM, the device's pose (position and orientation) x, and the positions (but not orientation) of various landmarks (loosely defined as things that can be identified from camera images) m_i--both in the world frame are captured as the state vector 𝛍^true = [x^true; m^true₁; ...; m^true_N], or more compactly 𝛍^true = [x^true; m^true].  Since I don't know where the device or the landmarks are initially my initial state estimate is 𝛍[0] = [0; 0].  Most text will annotate this vector with a hat (^) for the estimate, but again due to the Formatting constraint in the Blogger editor, I will just omit the hat, and instead denote the (unknowable) as ^true, as I've done above.

Ignoring for now the management of the landmark associations (they are sometimes unobservable as the device moves around the world frame), it is clear that the initial covariance matrix 𝚺_mm[0] = diag(∞, ..., ∞) because at the outset I know nothing about the landmark positions--except the landmark positions are independent of each other, and the device position, for that matter.  So 𝚺_𝛍𝛍[0] = diag(0, 0, 0, ∞, ..., ∞).  Note that this initialization of the device pose covariance (setting to 0) is different than what may be found in other Kalman filter textbooks (which uses ∞) because of the different initialization method in the book (coming up shortly below).  In linear (the original) Kalman filter formulation, the state evolution and observable generation are linear stochastic processes, but in EKF, they are Gaussian nonlinear processes (that can nevertheless be linearized):

	Kalman Filter	Extended Kalman Filter
Process (state change) model	𝛍true = A 𝛍true + B u + r	𝛍true = g(𝛍true, u) + r
Measurement model	zk[k]= H 𝛍true + qk[k]	zk[k]= hk(𝛍true) + qk[k]

where  r ~ 𝓝(0, R[k]) and q_k[k] ~ 𝓝(0, Q_k[k]).  The subscripts k in the measurement model is necessary in SLAM because landmarks come and go at every time step: to me, it means "pertaining to all observations at time step k".

The zero initial value of the states pretty much assure that the prediction of the state for the next time step is still zero--unless the system is such that zero is not an equilibrium, or if there is an external input AKA control input.  In this example of a planar robot movement using odometer model, there is apparently a non-zero initial external input (the odometry data):

u[0] = [rot1; trans; rot2] = [0.1007    0.1001    0.0002]^T.

Prediction step

When first studying Kalman filter, I was confused whether to perform prediction right after outputting the control command, or at the beginning of the next step, right before processing the fresh measurement.  Because of my background in real-time control, I perform the non-time-critical tasks such as prediction after outputting the control command (i.e. minimize time delay between sensor measurement available and when the control command is output).  But in this case, the external input is not the control command but rather another sensor reading (odometry reading), which is only available at the next time step.  That is why the "prediction" step uses the a-posteriori estimate from the last time step x⁺[k-1] = [x⁺[k-1]; y⁺[[k-1]; 𝛉⁺[k-1] ], which in this example:

𝛍^-[k] = g(u[k]; 𝛍⁺[k-1])

For additional clarity, I adopted the "-" and "+" superscripts for the a-priori and a-posteriori estimates.

Even though g(u[k]; 𝛍⁺[k-1]) can be a BHF (big hairy function) in general, stationarity assumption for the landmarks (m[k] = m[k-1]) in SLAM simplifies the state update to only the device pose update:

𝛍^-[k] = 𝛍⁺[k-1] + [trans * cos(𝛉⁺[k-1] + rot1) ; trans * sin(𝛉⁺[k-1] + rot1) ; rot1 + rot2]

Observation prediction: has to wait till landmark observation in SLAM

We will see shortly the a-posterior update (correction) driven by the sensor measurement (observation).  The prediction of the observation h(𝛍[k]) is only necessary at that time, but in the spirit of calculating everything possible outside the time critical path from sensor measurement being available to outputting control command, I want to pre-calculate h(𝛍[k]) now--except that in SLAM, I don't know a-priori which landmarks I will observe, so I have to wait till discovering all observations for the time step.

Covariance prediction

To update the covariance, I need the Jacobian matrix G^-[k]:

G^-[k] = ∂𝛍[k] / ∂𝛍[k-1] @ 𝛍^-[k] = ∂g(u[k]; 𝛍[k-1]) / ∂𝛍[k-1] @ 𝛍^-[k]

Note that the partial derivative is derived from the prediction (i.e. without the new sensor input), AT 𝛍[k-1]--the last optimal state estimate.  Once again, the landmark stationarity and independence assumption allows simplification:

G^-[k] = diag(G^-_x[k], I_2N); where G^-_x[k] = ∂x[k] / ∂x[k-1] @ x^-[k]

In the prediction equation above, only 𝛉 affects change of x, so that only the dx/d𝛉 and dy/d𝛉 need to be considered for the Jacobian calculation:

dx/d𝛉 = -trans * sin(𝛉 + rot1); dy/d𝛉 = trans * cos(𝛉 + rot1)

G^-_x[k] = I₃ + trans * [ -sin(𝛉^-[k] + rot1) ; cos(𝛉^-[k] + rot1) ; 0]

I think it is helpful to remember separation of the device pose and the landmark positions manifest in the covariance matrix structure as well:

𝚺_𝛍𝛍 = [𝚺_xx 𝚺_xm ; 𝚺^T_xm 𝚺_mm], since 𝚺_mx = 𝚺^T_xm.

Covariance prediction step involves this huge matrix (dimension (3+2N)² in this example):

𝚺^-_𝛍𝛍[k] = G^-[k] 𝚺⁺_𝛍𝛍[k-1] G^-T[k] + R[k]

In R[k] = diag(R^-_x[k], 0), R^-_x characterizes the random noise in the device pose (imagine the device being knocked around by an accident), evaluated at 𝛍^-[k]. I have not seen a noise covariance modeled with such fidelity as to be sensitive to the current state estimate, but wanted clarity for now.  Note that unless the physical process has no noise (i.e. R[k] is 0: impossible!), the state uncertainly always grows during the prediction step, as you can see in the following plot of the robot location estimate overlaid against the known landmarks.

Red: prior device pose estimate and uncertainly (a-posteriori estimate from previous time step)
Magenta: predicted device pose and uncertainty, which is larger than the red ellipse

In SLAM, the huge covariance prediction calculation above in the first term above can be broken up into 3 isolated portions (the off-diagonal terms are transpose of each other) for simplicity.

G-[k] 𝚺+𝛍𝛍[k-1] G-T[k] =	G-x[k] 𝚺+xx[k-1] G-T[k]	, G-x[k] 𝚺+xm[k-1]
	(G-x[k] 𝚺+xm[k-1])T	, 𝚺+mm[k-1]]

In fact, the independence of the landmarks allows even more factoring of G^-[k] 𝚺^-_𝛍𝛍[k-1] G^-T[k]:

G-x[k] 𝚺+xx[k-1] G-T[k]	, G-x[k] 𝚺+x1[k-1]	, ...	, G-x[k] 𝚺+xN[k-1]
(G-x[k] 𝚺+x1[k-1])T	, 𝚺+11[k-1]	, ...	, 𝚺+1N[k-1]
...	, ...	, ...	, ...
(G-x[k] 𝚺+xN[k-1])T	, 𝚺+N1[k-1]	, ...	, 𝚺+NN[k-1]

This formulation better shows the correlation between the device pose and individual landmark, and the inter-landmark correlation; it might be a better suited to the when landmarks rapidly come in and out of view, but might be an overkill for the simple case of fixed and unchanging landmarks.

Correction step

In this example problem, the range and bearing of a subset of the known landmarks are observed at each time step.  Assuming that the association problem (which landmark is this for?) is solved somehow, we can calculate the difference between the measured observation and the predicted observation, which is called innovation.  If a given landmark was previously observed, its estimated position is already in 𝛍^-[k].  But a newly discovered landmark i's state m^-_i[k] is initialized with yet another function:

m^-_i[k] = initial(𝛍^-[k], z_i[k])

There doesn't seem to be a lot of discussion on this initialization function, but if we can initialize, then we can use the completed 𝛍^-[k] to calculate the predicted observation h_p(𝛍^true) and the measurement Jacobian H_p[k], which is the gradient of the observables z[k] WRT 𝛍^true:

H_p[k] = ∂h_p(𝛍[k]) / ∂𝛍[k] @ 𝛍^-[k]

Just like the process Jacobian, there are many ways to estimate the measurement Jacobian (analytical, numerical, and their respective children), but I've only used analytical models so far.

State correction

K_p[k] is the optimal gain under the zero-mean Gaussian noise hypothesis--called Kalman gain.

K_p[k] = 𝚺^-_𝛍𝛍[k] H_p^T[k] / (H_p[k] 𝚺^-_𝛍𝛍[k] H_p^T[k] + Q_p[k])

Note that the "/" operator is in fact matrix inversion.  I find Kalman "weight" to be a better reminder of its true nature, because that is how much weight is given to the innovation z[k] - h_p(𝛍^-[k]) to update the state estimate:

𝛍⁺[k] = 𝛍^-[k] + K_p[k] ( z[k] - h_p(𝛍^-[k]) )

Note that the unobserved landmarks do not contribute to correcting the state estimate.

Covariance correction

The Kalman weight also contributes to the state covariance correction:

𝚺⁺_pp[k] = (I_pp - K_p[k] H_p[k]) 𝚺^-_pp[k] = 𝚺^-_pp[k] - K_p[k] H_p[k] 𝚺^-_pp[k]

where 𝚺_pp = [𝚺_xx 𝚺_xp ; 𝚺^T_xp 𝚺_pp] is the combination of the device pose to device pose covariance, device pose to observed (at k) landmark positions, and the landmark position to landmark position covariance.  Note that the covariance decreases commensurate to the Kalman weight, in reverse of the always increasing covariance during the prediction step earlier.

Red: predicted device pose estimate and uncertainly (a-priori estimate from this time step)
Magenta: corrected device pose and uncertainty, which is smaller than the red ellipse
Blue: predicted landmark position estimate and uncertainty (a-priori estimate from this time step)
Cyan: corrected landmark position and uncertainty, which is smaller than the blue ellipse
Black: range and bearing measurements at that time step, drawn from the corrected device pose (maybe I should have drawn it from the predicted device pose)

This completely textbook implementation of EKF leaves much to be desired.  I had to futz around with the sensor and process uncertainties to avoid gross estimation error--to some extent, this is one of the deficiencies of KF I already heard about before.  But I was surprised by how much the solution drifts as the device moves away from the starting position, which is in the lower left corner below.

If I reduced the sensor covariance too much, the final landmark (at 9,2) doesn't even fall within the 2𝛔  ellipse.   The drift might be due to the sensor bias, and probably uncorrectable without another sensor or a loop closure (coming later in the course).

There is much, much more to Kalman filter

While reading GNSS books about a year ago, I realized that the Kalman filter that I and many other people know is just the tip of the iceberg, and I am not even talking about the UFK.  A year ago, I could not have imagined such things as the Kalman smoother, closed-loop Kalman filter, or constrained Kalman filter.  I hope to share my future discoveries on such topics with you.

Dual system architecture for Raspberry PI

Because I first started learning about programming in a real-time context, I never became comfortable with the idea of controlling physical (safety-critical) devices on a non-deterministic OS.  So I stayed with MCUs (Microcontroller) for a long time.  On the other hand, as I tried to build shipping products, I became painfully aware of the whole infrastructure necessary to control and communicate with the MCU, and display the relevant information to the human operator or remote systems.  I realized that a big chunk of my paycheck was justified because I had the patience and skills to do this mundane but necessary chores for the companies I worked in.  In fact, when I was in the biotech industry, my fellow engineers often viewed ourselves as plumbers (although not as well paid).  But in my first foray into the world of SoC programming on Xilinx Zynq, I came up with a solution to the problem of integrating a general OS (running UI) and a bare-metal system by running Linux on CPU0 of Zynq, and running the bare-metal firmware on CPU1 of Zynq.  I later learned that my solution is being used at CERN and Bosch.  The solution hinges on the OCM (on-chip-memory) and the software interrupt available on the Zynq, as you can see below:

The interrupt controller is part of ARMv7 architecture, but OCM is Xilinx's contribution.  The 2 systems can even share data that is too big to fit in the relatively tiny OCM; I even prototyped a custom image collection system that takes images from an Aptina (now On Semi) CCD scheduled by the bare metal, that Linux can read later from the shared DRAM area, using Xilinx VDMA core, as explained in my previous blog entry.

But since leaving the biotech industry, I haven't touched FPGA.  Instead, I have worked with ARM Cortex M (even got Linux to run on ARM Cortex M4!), FreeRTOS and Raspberry PI (3).  In the last year, I was too busy studying DSP and computer vision, but I am beginning to itch for low level programming again.  I was blown away by Raspberry PI's bang-for-the-buck (it even has a programmable GPU, which you can use as DSP), so this time, I think I will ditch my $600 Zedboard in favor of the $35 RPI3 I bought more than a year ago.  The only complaint I have is the low performance of the data pipe from the CPU to the GPU, but this is a common problem in ALL computation pipeline that includes GPU (including CUDA).  I have some hope that ARM will solve the problem by more tightly integrating Mali with ARM Cortex A series in the future, but let's not let that stop us.

So if the OCM was the key to the dual system in my solution on Zynq, and BCM2837 (the SoC for RPI3) is MISSING an equivalent HW, how shall I work around it?  I first thought about using the DRAM as the shared memory, but I am concerned about degrading the cache hit, which I want to keep high for the hard real-time code still running out of DRAM.  Since the message path between the real-time and the non-real-time system does NOT have to be real-time, I am going to use another HW to shuttle the message, as shown below.

RPI3 only has 1 USART, so I cannot directly connect the 2 halves.  But since Linux (and U-Boot also, I think) is capable of using USBserial console out of the box (it appears as a /dev file), there should be little work necessary to communicate with the bare-metal code--if the bare-metal can read/write to the BCM2837's USART peripheral.  The current maximum baud rate supported by the USBserial IC category leader FT232R is 3 Mbps, which is fast enough to shuttle most messages into/out of the real-time system--except images.  So this architecture requires keeping image processing algorithms on Linux--which is frankly just easier (because you can't port OpenCV to bare metal).

Another change from my previous solution is to remove the capability to start/stop the real-time subsystem from Linux--at least for now.  I heard from Bosch that safety-critical real-time program needs to be available quickly from power-on, so the ~10 second delay when being launched by Linux would be unacceptable.  So this time, U-Boot is going to first kick off the bare metal program on CPU3, and THEN start Linux (before exiting).

I lost my Linux development system when I exited the biotech industry, so I just installed Parallels, to begin recreating my Buildroot development environment.  Because I am busy studying a few other things (on top of my full time job), a fully working demo is going to take some time to materialize.  If you know how to do the following, you should just be able to pull it off by yourself:

1. Building and running a Buildroot distribution on RPI.
2. Booting multiple systems (in sequence) from U-Boot.
3. Configuring Linux to run on only subset of the CPUs, and using only a portion of the physical DRAM.
4. Writing a bare-metal (or FreeRTOS) system that runs out of the reserved portion of the DRAM (with L1/L2 cache turned on).

Understanding Doppler phase

I wanted to fuse the real-time mic recording on my iPhone with the accelerometer/gyro signals for my hobby indoor location project.  But for reasons I discussed in my previous blog entry, I am currently stuck.  To get unstuck, I started thinking about the range rate estimation used in radars, which are primarily Doppler based.  Doppler frequency shift is covered in high school physics, but it's been a long time since I've had to think about Doppler, so I derived it out myself.

Consider a mic M initially at rest, being subject to a sound wave of frequency F, and wave speed c, as shown below.  For convenience, let's pick the beginning of sample to coincide with phase 0 of the sine wave.  This is just picking the constant offset phase, so using 0 initial phase does not invalidate the ensuing derivation.

Since the mic is at rest (v_M = 0), the sound wave passes by the mic at c [m/s].  After t [s] passes, the mic will experience the phase 𝛟 = 2𝛑Ft of the sound wave, as shown below.

Now suppose the mic is moving at v [m/s].  Then the situation is exactly analogous to a race between the mic and the sound wave; so relative to the mic, the sound wave is moving slower than when the mic was stationary, as shown below.

How much slower?  After the same t [s], the wave phase will have moved -2𝛑Ft * v/c less than previous case, so that the mic is now experiencing the phase 𝛟 = 2𝛑F(1-v/c)t.  When I divide the difference of phase between the 2 cases -2𝛑t * v/c by the elapsed time t, I get -2𝛑F v/c, which is exactly the Doppler frequency shift.  This also make sense because the derivative of phase is frequency.

This derivation might be helpful if you want to measure Doppler from the raw waveform rather than by measuring the received wave frequency, as in radar processing.  The problem I find with frequency based estimation is that I cannot get the instantaneous frequency; since frequency requires several waveforms to estimate (the more the more accurate).  So while the Doppler from frequency domain processing may be fine in an average sense, I have to give up some resolution in the process.  Since radar waves are typically microwave frequency, requiring lots of periods is not a problem for radar applications (even after the signal is heterodyned and down-sampled).  But for sound waves sampled at CD sample rate, requiring say 20 periods (I would not go below due to noise concerns) of 10 kHz wave would equate to 10 / F = 2 ms, which may start to be a problem for high dynamics applications.
Estimating the wave phase directly as a function of the range rate might also be helpful for estimating the distance change directly from the raw audio signal.  I actually just read The Doppler Equation in Range and Range Rate Measurement, NASA Technical Note X-55373, dating back to 1965 (!) for the Apollo program, where the debate seems to have been over how best to track both the range and range rate of the rocket.  Even though the author (NASA Goddard) seems to have decided in favor of obtaining the range by integrating the estimated range rate (from Doppler), you might find the direct estimation of the range (without the random walk noise inducing integration step) useful.  In the next month, I hope to find some time to implement a Kalman filter to do just that.

Understanding sorting network in Matlab

When I began this hobby project to solve the indoor location problem using only the HW available on a modern smartphone--all just to play an augmented reality X-wing vs. Tie-figher game with my friend's 5-year old son, I had no idea how much I would have to figure out.  And it's turning out to be quite a lot: the definition of "knowing" takes on a whole new dimension when you try to make something that works.  But it turns out this is the best way I learn; I should have tried to make something by myself a long time ago!

Now that I understand how to get continuous correlation from the matched filter (as explained in my previous blog entry), I am trying to boost the SNR of the filter and fish out the instantaneous jump in correlation when there is a chirp.  As shown in the last blog entry, that spike can be buried in the ambient sound (usually of much lower frequency than the near-Nyquist frequency of the correlation spike).  One of my favorite techniques is to subtract the median of rolling time window: the median is superior to the mean in smoothing a signal in a high noise environment.  The only problem is that rolling median is hard to compute, unlike the rolling mean.  The most naive implementation will sort the content of the rolling window at every sample point.  Even with an O(N logN) sorting algorithm, the cost of sorting goes up quickly as the window size grows.  More sophisticated running median algorithm tries to keep an ordered rolling window.  I actually tried to do this with a doubly linked list, with a special pointer to the median, as shown in the sketch below.

Since I use a circular queue in all embedded projects, I was going to maintain a doubly linked circular buffer of size 1 greater than the window size, so that I could make the oldest element stale, and decide whether to move the median to the left or to right (or leave as is) depending on the combination of the values of the elements, as hinted at below.

This took me 2 nights of coding and debugging to get right.  Incidentally, problems like this come up often in SW interviews.  Here is the class that does all the heavy lifting:

template <typename T> class SortedQ {

    class dValue {

        friend class SortedQ;

    protected:

        dValue *_prev, *_next;

    public:

        T val;

        bool stale;

        inline void init(T v) { stale = false; val = v; }

        inline dValue* next() {

            return (!_next || !_next->stale) ? _next : _next->_next;

        }

        inline dValue* prev() {

            return (!_prev || !_prev->stale) ? _prev : _prev->_prev;

        }

        static int compare(const void* lp, const void* rp) {

            const dValue* *l = (const dValue**)lp;

            const dValue* *r = (const dValue**)rp;

            return (*l)->val - (*r)->val;

        }

        inline void rlink(dValue& right) { _next = &right; right._prev = this; }

        inline void llink(dValue& left) { _prev = &left; left._next = this; }

        inline void replace(dValue* old) {

            _prev = old->_prev; _next = old->_next;

            _prev->_next = _next->_prev = this;

        }

        void rinsert(dValue* newnode);

        void linsert(dValue* newnode);

        inline void dropout() {

            if (_next) _next->_prev = _prev;

            if (_prev) _prev->_next = _next;

        }

    };

public:

    static constexpr unsigned SIZE = 5;

    dValue arr[SIZE+1];

    SortedQ() {

        memset(arr, 0, sizeof(arr)); // mass-set pointers to 0

    }

    ~SortedQ() = default;

    void init();

    inline T& median() { return _median->val; }

    

    //Tail is where we will insert the NEXT sample to, so

    //head should be the slot AFTER the tail (circular).

    inline dValue* head() { return &arr[(_tail + 1) % (SIZE+1)]; }

    T& push(T& f);

    void print();

private:

    unsigned _tail;//In stable state, _tail points to the NEXT slot

    dValue* _median;

};

As evidenced by my slow coding speed, I think I am not a top-notch SW developer--especially given that my code assumes non-duplicate elements.  If I piqued your interest by tying this to SW interviews, the best known solution is to keep 2 separate ordered lists (that can report its size) on either side of the median, and to keep the 2 sides of equal length at all times.  A simple list has O(N) search and insertion time, so people use advanced data structures like balanced binary tree, or even skip list, for the O(logN) insertion and search time.  I can guarantee that such implementation will not be requested in a 45 minute SW interview!

So even for a seemingly simple task of keeping a sorted window, SW solution will be complicated; and I hate complexity.   As someone who is more interested in HW than the SW, there is bigger disappointment in the SW solution discussed above: there is no HW optimization!  The CPU is spending most of its time managing pointers, and the actual value comparison is only a minuscule portion of the total running time.  But in THIS problem, where the window size is fixed and known at design time, and the data is streaming in nature, there IS one important HW optimization possible: vector-wise sorting using sorting network.  The streaming nature of the data means that the sliding window of successive samples look like delayed version of the previous window, as illustrated below:

I can batch-sort a block of inputs going through this sorting window, by treating each delay (starting at delay 0) as a separate vector.  For the window size of 5, for example, there are 5 inputs going into my sorting network--which has a known optimum solution (both in number of comparisons and delay) discussed in The Art of Computer Programming, Vol 3 and reproduced on Ron Zeno's website.

While the sorting network is ideally implemented in a custom HW, the next best implementation can be had on SIMD capable chips, like on Intel with SSE, or ARM with Neon.  And the vectored nature of Matlab's max/min function can easily simulate it.  Here's my implementation of 5-element sorting network:

There are 2 opportunities for parallelization: at the very begging and right before the last swapping.  I deliberately drew them staggered because I am NOT going to parallelize them.  The letters in the middle show the temporary vectors necessary to store the intermediate values--unless you have a HW assisted compare and swap--because each compare and swap needs to be done in 2 stages: taking the max of 2 vectors, and then taking the min of the same 2 vectors.  Right away, you can see that the vectored sorting network will be memory hungry, and might not be suitable for cheap MCUs.  But an iPhone can easily provide a few extra KB without throwing up.

Here's a Matlab simulation I used to verify this network:

x = rand(1000,1);

d1 = [x(1); x(1:end-1)];

d2 = [d1(1); d1(1:end-1)];

d3 = [d2(1); d2(1:end-1)];

d4 = [d3(1); d3(1:end-1)];

plot(x); hold on;

plot(d4);

hold off;

A = max(x, d1);  B = min(x, d1);

C = max(d2, d3); D = min(d2, d3);

E = max(B, D);   D = min(B, D);

B = max(C, d4);  C = min(C, d4);

F = max(A, B);   B = min(A, B);

A = max(E, C);   C = min(E, C);

E = max(A, B);   B = min(A, B);

D = max(D, C);

B = max(B, D);

truth = medfilt1(x, 5);

med_err = truth(1:end-2) - B(3:end);

plot(x); hold on; 

plot(truth(1:end-2)); plot(B(3:end)); hold off;

plot(med_err);

You can see in my previous blog entry that I used 3 extra arrays of length 1K (that's the length of the matched filter) for correlation processing.  That is already 3 x 1K x 4B = 12 KB of RAM.  For this median processing, I can reuse those 3 arrays on the stack for A, B, and C.  But I still have to come up with D and E (F is only necessary to get the running maximum), so I am looking at 20 KB extra RAM for the correlation and median processing.  As a reference, nRF51822 the BLE MCU I am used to playing around with only has up to 32 KB of RAM, so this solution is starting to look expensive...