function [EstErrK, EstErrUs, EstErrU] = HybridSimplex(SphericalFlag) % Optimal State Estimation, by Dan Simon % % Hybrid unscented Kalman filter example with simplex sigma points or spherical sigma points. % Track a body falling through the atmosphere. % This example is taken from [Jul00], which was based on [Ath68]. % INPUTS: % SphericalFlag = flag indicating whether or not to use spherical sigma % points (if false, then use simplex sigma points) % OUTPUTS: % EstErrK = Kalman filter RMS est error for position, velocity, ballistic coefficient % EstErrUs = Standard UKF RMS est error for position, velocity, ballistic coefficient % EstErrU = Minimal (spherical or simplex) UKF RMS est error for position, velocity, ballistic coefficient if ~exist('SphericalFlag', 'var') SphericalFlag = true; end rho0 = 2; % lb-sec^2/ft^4 g = 32.2; % ft/sec^2 k = 2e4; % ft R = 10^4; % measurement noise variance (ft^2) Q = diag([0 0 0]); % process noise covariance M = 10^5; % horizontal range of position sensor a = 10^5; % altitude of position sensor x = [3e5; -2e4; 1000]; xhat = 1.01 * [3e5; -2e4; 1000]; % EKF estimate xhatukf = xhat; % simplex or spherical UKF estimate xhatukfs = xhat; % standard UKF estimate P = diag(0.1*abs(x)); Pukf = P; Pukfs = P; T = 0.5; % measurement time step randn('state',sum(100*clock)); % random number generator seed tf = 30; % simulation length (seconds) dt = 0.001; % time step for integration (seconds) xArray = x; xhatArray = xhat; xhatukfArray = xhatukf; xhatukfsArray = xhatukfs; N = length(x); % Generate the weights for the simplex or spherical UKF. if SphericalFlag W(1) = 0; W(2) = (1 - W(1)) / (N + 1); W(3) = W(2); W(4) = W(3); W(5) = W(4); else W(1) = 0; W(2) = (1 - W(1)) / 2^N; W(3) = W(2); W(4) = 2 * W(3); W(5) = 2 * W(4); end % Generate the sigma points for the simplex or spherical UKF. % Initialization sigma = zeros(3,5); sigma(1,1) = 0; sigma(1,2) = -1 / sqrt(2 * W(2)); sigma(1,3) = 1 / sqrt(2 * W(2)); % j = 2 iteration j = 2; if SphericalFlag sigma(1:j,1) = [sigma(1:j-1,1) ; 0]; sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(j*(j+1)*W(2))]; sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(j*(j+1)*W(2))]; sigma(1:j,4) = [zeros(j-1,1) ; j/sqrt(j*(j+1)*W(2))]; else sigma(1:j,1) = [sigma(1:j-1,1) ; 0]; sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(2*W(j+2))]; sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(2*W(j+2))]; sigma(1:j,4) = [zeros(j-1,1) ; 1/sqrt(2*W(j+2))]; end % j = 3 iteration j = 3; if SphericalFlag sigma(1:j,1) = [sigma(1:j-1,1) ; 0]; sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(j*(j+1)*W(2))]; sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(j*(j+1)*W(2))]; sigma(1:j,4) = [sigma(1:j-1,4) ; -1/sqrt(j*(j+1)*W(2))]; sigma(1:j,5) = [zeros(j-1,1) ; j/sqrt(j*(j+1)*W(2))]; else sigma(1:j,1) = [sigma(1:j-1,1) ; 0]; sigma(1:j,2) = [sigma(1:j-1,2) ; -1/sqrt(2*W(j+2))]; sigma(1:j,3) = [sigma(1:j-1,3) ; -1/sqrt(2*W(j+2))]; sigma(1:j,4) = [sigma(1:j-1,4) ; -1/sqrt(2*W(j+2))]; sigma(1:j,5) = [zeros(j-1,1) ; 1/sqrt(2*W(j+2))]; end Ws = ones(2*N,1) / 2 / N; % standard UKF weights for t = T : T : tf % Simulate the system. for tau = dt : dt : T xdot(1,1) = x(2); xdot(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 / x(3) - g; xdot(3,1) = 0; xdot = xdot + sqrt(dt * Q) * [randn; randn; randn]; x = x + xdot * dt; end % Simulate the noisy measurement. z = sqrt(M^2 + (x(1)-a)^2) + sqrt(R) * randn; % Simulate the continuous-time part of the Kalman filter (time update). for tau = dt : dt : T xhatdot(1,1) = xhat(2); xhatdot(2,1) = rho0 * exp(-xhat(1)/k) * xhat(2)^2 / 2 / xhat(3) - g; xhatdot(3,1) = 0; xhat = xhat + xhatdot * dt; temp = rho0 * exp(-xhat(1)/k) * xhat(2) / xhat(3); F = [0 1 0; -temp * xhat(2) / 2 / k temp ... -temp * xhat(2) / 2 / xhat(3); ... 0 0 0]; H = [(xhat(1) - a) / sqrt(M^2 + (xhat(1)-a)^2) 0 0]; Pdot = F * P + P * F' + Q * dt - P * H' * inv(R) * H * P; P = P + Pdot * dt; end % Simulate the discrete-time part of the Kalman filter (measurement update). H = [(xhat(1) - a) / sqrt(M^2 + (xhat(1)-a)^2) 0 0]; K = P * H' * inv(H * P * H' + R); zhat = sqrt(M^2 + (xhat(1)-a)^2); xhat = xhat + K * (z - zhat); P = (eye(N) - K * H) * P * (eye(N) - K * H)' + K * R * K'; % Start of simplex / spherical UKF - matrix square root calculation [root,p] = chol(Pukf); for i = 1 : N+2 xbreve(:,i) = xhatukf + root * sigma(:,i); end % Simplex / spherical UKF time update for i = 1 : N+2 for tau = dt : dt : T xbrevedot(1,1) = xbreve(2,i); xbrevedot(2,1) = rho0 * exp(-xbreve(1,i)/k) * xbreve(2,i)^2 / 2 / xbreve(3,i) - g; xbrevedot(3,1) = 0; xbreve(:,i) = xbreve(:,i) + xbrevedot * dt; end end xhatukf = zeros(N,1); for i = 1 : N+2 xhatukf = xhatukf + W(i) * xbreve(:,i); end Pukf = Q; for i = 1 : N+2 Pukf = Pukf + W(i) * (xbreve(:,i) - xhatukf) * (xbreve(:,i) - xhatukf)'; end % Simplex / spherical UKF measurement update for i = 1 : N+2 zukf(:,i) = sqrt(M^2 + (xbreve(1,i)-a)^2); end zhat = 0; for i = 1 : N+2 zhat = zhat + W(i) * zukf(:,i); end Py = R; Pxy = zeros(N,1); for i = 1 : N+2 Py = Py + W(i) * (zukf(:,i) - zhat) * (zukf(:,i) - zhat)'; Pxy = Pxy + W(i) * (xbreve(:,i) - xhatukf) * (zukf(:,i) - zhat)'; end Kukf = Pxy * inv(Py); xhatukf = xhatukf + Kukf * (z - zhat); Pukf = Pukf - Kukf * Py * Kukf'; % Start of standard UKF - generate the UKF sigma points. [root,p] = chol(N*Pukfs); for i = 1 : N sigmas(:,i) = xhatukfs + root(i,:)'; sigmas(:,i+N) = xhatukfs - root(i,:)'; end for i = 1 : 2*N xbreve(:,i) = sigmas(:,i); end % Standard UKF time update for i = 1 : 2*N for tau = dt : dt : T xbrevedot(1,1) = xbreve(2,i); xbrevedot(2,1) = rho0 * exp(-xbreve(1,i)/k) * xbreve(2,i)^2 / 2 / xbreve(3,i) - g; xbrevedot(3,1) = 0; xbreve(:,i) = xbreve(:,i) + xbrevedot * dt; end end xhatukfs = zeros(N,1); for i = 1 : 2*N xhatukfs = xhatukfs + Ws(i) * xbreve(:,i); end Pukfs = Q; for i = 1 : 2*N Pukfs = Pukfs + Ws(i) * (xbreve(:,i) - xhatukfs) * (xbreve(:,i) - xhatukfs)'; end % Standard UKF measurement update for i = 1 : 2*N zukf(:,i) = sqrt(M^2 + (xbreve(1,i)-a)^2); end zhat = 0; for i = 1 : 2*N zhat = zhat + Ws(i) * zukf(:,i); end Py = R; Pxy = zeros(N,1); for i = 1 : 2*N Py = Py + Ws(i) * (zukf(:,i) - zhat) * (zukf(:,i) - zhat)'; Pxy = Pxy + Ws(i) * (xbreve(:,i) - xhatukfs) * (zukf(:,i) - zhat)'; end Kukf = Pxy * inv(Py); xhatukfs = xhatukfs + Kukf * (z - zhat); Pukfs = Pukfs - Kukf * Py * Kukf'; % Save data for plotting. xArray = [xArray x]; xhatArray = [xhatArray xhat]; xhatukfArray = [xhatukfArray xhatukf]; xhatukfsArray = [xhatukfsArray xhatukfs]; end EstErrK = std(xArray' - xhatArray'); EstErrU = std(xArray' - xhatukfArray'); EstErrUs = std(xArray' - xhatukfsArray'); disp(['RMS altitude est err = ', num2str(EstErrK(1)), ' (EKF), ', num2str(EstErrUs(1)), ... ' (Std UKF), ', num2str(EstErrU(1)), ' (Minimal UKF)']); disp(['RMS velocity est err = ', num2str(EstErrK(2)), ' (EKF), ', num2str(EstErrUs(2)), ... ' (Std UKF), ', num2str(EstErrU(2)), ' (Minimal UKF)']); disp(['RMS ball coeff est err = ', num2str(EstErrK(3)), ' (EKF), ', num2str(EstErrUs(3)), ... ' (Std UKF), ', num2str(EstErrU(3)), ' (Minimal UKF)']);