from
Kalman filtering framework
by David Ogilvie
Object framework for filtering using Kalman filter, EKF, or UKF.
|
| symmetricSigmaPoints(n, W_mean, isScaled, alpha, beta) |
function [W_s, W_c, sigmaOffsets] = symmetricSigmaPoints(n, W_mean, isScaled, alpha, beta)
% input:
% n = dimension of state
% W_mean = real number between 0 and 1 = weight assigned to term
% f(xbar), where xbar is the mean.
% outputs:
% W_s = weights for state vector
% W_c = weights for covariance matrix
% points = cubature points
% symmetric sigma points
if n == 0 % degenerate case where we want exact value for f(x) (i.e. covariance == 0)
W(1) = 1;
sigmaOffsets = zeros(0, 1);
else
W(1) = W_mean;
W(2:2*n+1) = (1-W_mean)/(2*n);
sigmaOffsets = zeros(n, 2*n+1);
sigmaOffsets(:, 2:n+1) = sqrt(n/(1 - W_mean)) * eye(n);
sigmaOffsets(:, n+2:2*n+1) = -sqrt(n/(1 - W_mean)) * eye(n);
end
if isScaled == false
W_s = W;
%W_c = diag(W);
W_c = W;
else
W_s(1) = W(1) / alpha^2 + (1 - 1/alpha^2);
W_s(2:2*n+1) = 1/alpha^2 * W(2:2*n+1);
W_c = W_s;
W_c(1) = W_s(1) + (1 - alpha^2 + beta);
%W_c = diag(W_c);
sigmaOffsets = alpha * sigmaOffsets;
end
end
|
|
Contact us at files@mathworks.com