130 lines
4.7 KiB
Matlab
130 lines
4.7 KiB
Matlab
function [x, S, K, inov, x_kkm1, S_kkm1, P_xy, S_yy] = srcdkf_opt(t_km1, t_k, x_km1, u_km1, S, y_k, Sv, Sn, h, robotName, Geometry, Gravity, integrationAlgorithm, dt_control, Xd, Kp, Ki, Kd, Ktau, antiWindup, limQ_L, limQ_U, limQp_L, limQp_U, limQpp_L, limQpp_U, limTau_L, limTau_U, useComputedTorque)
|
|
%#codegen
|
|
|
|
% CDKF: Square Root Central Difference Kalman Filter
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
|
% This code is inspired of the work of Wan, Eric A. and Rudoph van der Merwe
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% INPUTS:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% x_km1: state mean at time k-1
|
|
% S_km1: matrix square root of state covariance at time k-1
|
|
% u_km1: control input at time k-1
|
|
% y_k: noisy observation at time k
|
|
% Sv: matrix square root of process noise covariance matrix
|
|
% Sn: matrix square root of observation noise covariance matrix
|
|
|
|
% SRCDKF tunning parameters:
|
|
% h >= 1 scalar central difference interval size
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% OUTPUTS:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% x: estimated state
|
|
% S: matrix square root of estimated state covariance
|
|
% K: Kalman Gain
|
|
% inov: inovation signal
|
|
% x_kkm1: predicted state mean
|
|
% S_kkm1: matrix square root of predicted state covariance
|
|
% P_xy: predicted state and observation covariance
|
|
% S_yy: matrix square root of inovation covariance
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% Problem dimensions:
|
|
Xdim = length(x_km1); % Number of states
|
|
Vdim = size(Sv, 1); % Number of noise states
|
|
Ydim = size(Sn, 1); % Number of observations
|
|
L_v = Xdim + Vdim; % Dimension of augmented state
|
|
L_n = Xdim + Ydim; % Dimension of augmented state
|
|
Ns_v = 2*L_v+1; % Number of sigma points
|
|
Ns_n = 2*L_n+1; % Number of sigma points
|
|
|
|
% Weights:
|
|
h2 = h^2;
|
|
W_m_0_v = (h2 - L_v)/h2;
|
|
W_m_0_n = (h2 - L_n)/h2;
|
|
W_m_i = 1/(2*h2);
|
|
W_c1_i = 1/(2*h);
|
|
W_c2_i = sqrt(h2-1)/(2*h2);
|
|
|
|
%% Calculate sigma-points for time-update:
|
|
|
|
% Build the augmented system:
|
|
S_aug_v = blkdiag(S, Sv);
|
|
h_S_aug_v = h*S_aug_v;
|
|
x_aug_v_km1 = [x_km1; zeros(Vdim, 1)];
|
|
X_aug_v_km1 = [x_aug_v_km1, repmat(x_aug_v_km1, 1, L_v) + h_S_aug_v, repmat(x_aug_v_km1, 1, L_v) - h_S_aug_v];
|
|
|
|
%% Time-update equations:
|
|
|
|
X_x_kkm1 = zeros(Xdim, Ns_v);
|
|
state = X_aug_v_km1(1:Xdim, :);
|
|
pNoise = X_aug_v_km1(Xdim+1:Xdim+Vdim, :);
|
|
|
|
parfor sp = 1:Ns_v
|
|
X_x_kkm1(:, sp) = process_Model(t_km1, t_k, state(:,sp), u_km1, pNoise(:,sp), robotName, Geometry, Gravity, integrationAlgorithm, dt_control, Xd, Kp, Ki, Kd, Ktau, antiWindup, limQ_L, limQ_U, limQp_L, limQp_U, limQpp_L, limQpp_U, limTau_L, limTau_U, useComputedTorque);
|
|
end
|
|
|
|
% Expected prediction:
|
|
x_kkm1 = W_m_0_v * X_x_kkm1(:, 1) + W_m_i * sum(X_x_kkm1(:, 2:end), 2);
|
|
|
|
% Compute covariance of the prediction:
|
|
X_x_kkm1mx = W_c1_i * (X_x_kkm1(:,2:L_v+1)-X_x_kkm1(:,L_v+2:end));
|
|
X_x_kkm1px = W_c2_i * bsxfun(@minus,X_x_kkm1(:,2:L_v+1)+X_x_kkm1(:,L_v+2:Ns_v), 2*X_x_kkm1(:,1));
|
|
[~,S_kkm1] = qr([X_x_kkm1mx, X_x_kkm1px].',0);
|
|
S_kkm1 = S_kkm1.';
|
|
|
|
|
|
%% Calculate sigma-points for measurement-update:
|
|
|
|
% Build the augmented system:
|
|
S_aug_n = blkdiag(S_kkm1, Sn);
|
|
x_aug_n_km1 = [x_kkm1; zeros(Ydim, 1)];
|
|
h_S_aug_n = h*S_aug_n;
|
|
X_aug_n_km1 = [x_aug_n_km1, repmat(x_aug_n_km1, 1, L_n) + h_S_aug_n, repmat(x_aug_n_km1, 1, L_n) - h_S_aug_n];
|
|
|
|
%% Measurement-update equations:
|
|
|
|
Y_kkm1 = zeros(Ydim, Ns_n);
|
|
mState = X_aug_n_km1(1:Xdim, :);
|
|
mNoise = X_aug_n_km1(Xdim+1:Xdim+Ydim, :);
|
|
|
|
parfor sp = 1:Ns_n
|
|
Y_kkm1(:, sp) = measurement_Model(mState(:, sp), mNoise(:, sp));
|
|
end
|
|
|
|
% Expected measurement:
|
|
y_kkm1 = W_m_0_n * Y_kkm1(:, 1) + W_m_i * sum(Y_kkm1(:, 2:end), 2);
|
|
|
|
% Compute covariance of predicted measurement:
|
|
Y_kkm1mx = W_c1_i * (Y_kkm1(:,2:L_n+1)-Y_kkm1(:,L_n+2:end));
|
|
Y_kkm1px = W_c2_i * bsxfun(@minus,Y_kkm1(:,2:L_n+1)+Y_kkm1(:,L_n+2:Ns_n), 2*Y_kkm1(:,1));
|
|
[~,S_yy] = qr([Y_kkm1mx, Y_kkm1px].',0);
|
|
S_yy = S_yy.';
|
|
|
|
% Compute covariance of predicted observation and predicted state:
|
|
Syx1 = Y_kkm1mx(:,1:Xdim);
|
|
Syw1 = Y_kkm1mx(:,Xdim+1:end);
|
|
P_xy = S_kkm1*Syx1.';
|
|
|
|
% Compute innovation vector:
|
|
inov = y_k - y_kkm1;
|
|
|
|
% Compute the Kalman gain:
|
|
K = (P_xy/S_yy.')/S_yy;
|
|
|
|
% State correction:
|
|
x = x_kkm1 + K*inov;
|
|
|
|
% Covariance correction:
|
|
[~,S] = qr([S_kkm1-K*Syx1 K*Syw1 K*Y_kkm1px].',0); % Inspired by the Rebel toolkit implementation of Wan, Eric A. and Rudoph van der Merwe
|
|
S = S.';
|
|
|
|
end % srcdkf
|