175 lines
7.1 KiB
Mathematica
175 lines
7.1 KiB
Mathematica
|
function [x, P, K, inov, x_kkm1, P_kkm1, P_xy, P_yy] = cdkf_opt(t_km1, t_k, x_km1, u_km1, P, y_k, Rv, Rn, h, sigmaComputeMethod, 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: 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
|
||
|
|
% P_km1: state covariance at time k-1
|
||
|
|
% u_km1: control input at time k-1
|
||
|
|
% y_k: noisy observation at time k
|
||
|
|
% Rv: process noise covariance matrix
|
||
|
|
% Rn: observation noise covariance matrix
|
||
|
|
|
||
|
|
% CDKF tunning parameters:
|
||
|
|
% h >= 1 scalar central difference interval size
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
% OUTPUTS:
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
|
||
|
|
% x: estimated state
|
||
|
|
% P: estimated state covariance
|
||
|
|
% K: Kalman Gain
|
||
|
|
% inov: inovation signal
|
||
|
|
% x_kkm1: predicted state mean
|
||
|
|
% P_kkm1: predicted state covariance
|
||
|
|
% P_xy: predicted state and observation covariance
|
||
|
|
% P_yy: inovation covariance
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
|
||
|
|
% Problem dimensions:
|
||
|
|
Xdim = length(x_km1); % Number of states
|
||
|
|
Vdim = size(Rv, 1); % Number of noise states
|
||
|
|
Ydim = size(Rn, 1); % Number of observations
|
||
|
|
Ns_x = 2*Xdim+1; % Number of sigma points (first set)
|
||
|
|
Ns_v = 2*Vdim; % Number of sigma points (second set)
|
||
|
|
Ns_n = 2*Ydim; % Number of sigma points (third set)
|
||
|
|
|
||
|
|
% Weights:
|
||
|
|
h2 = h^2;
|
||
|
|
h4 = h^4;
|
||
|
|
W_m_0_v = (h2 - Xdim - Vdim)/h2;
|
||
|
|
W_m_0_n = (h2 - Xdim - Ydim)/h2;
|
||
|
|
W_m_i = 1/(2*h2);
|
||
|
|
W_c1_i = 1/(4*h2);
|
||
|
|
W_c2_i = (h2-1)/(4*h4);
|
||
|
|
|
||
|
|
%% Calculate sigma-points for time-update:
|
||
|
|
|
||
|
|
% Build the augmented system:
|
||
|
|
|
||
|
|
switch sigmaComputeMethod
|
||
|
|
case false
|
||
|
|
% Compute sqrt(P) with Cholesky factorization
|
||
|
|
[S_x, chk] = chol(P,'lower');
|
||
|
|
if chk > 0
|
||
|
|
error('CDKF: Error in Cholesky factorization at time update.')
|
||
|
|
end
|
||
|
|
h_S_x = h * S_x;
|
||
|
|
% Compute sqrt(Rv) with Cholesky factorization
|
||
|
|
[S_v, chk] = chol(Rv,'lower');
|
||
|
|
if chk > 0
|
||
|
|
error('CDKF: Error in Cholesky factorization at time update.')
|
||
|
|
end
|
||
|
|
h_S_v = h * S_v;
|
||
|
|
case true
|
||
|
|
% Compute sqrt(P) with SVD in case there is a 0 eigenvalue
|
||
|
|
[U_x, Sigma_x, V_x] = svd(P,0);
|
||
|
|
h_S_x = h * U_x * sqrt(Sigma_x) * V_x.';
|
||
|
|
% Compute sqrt(Rv) with SVD in case there is a 0 eigenvalue
|
||
|
|
[U_v, Sigma_v, V_v] = svd(Rv,0);
|
||
|
|
h_S_v = h * U_v * sqrt(Sigma_v) * V_v.';
|
||
|
|
otherwise
|
||
|
|
error("CDKF: Please select a correct sigma point computation method !");
|
||
|
|
end
|
||
|
|
|
||
|
|
X_km1 = [x_km1, repmat(x_km1, 1, Xdim) + h_S_x, repmat(x_km1, 1, Xdim) - h_S_x];
|
||
|
|
V_km1 = [h_S_v, -h_S_v];
|
||
|
|
|
||
|
|
%% Time-update equations:
|
||
|
|
|
||
|
|
X_x_kkm1 = zeros(Xdim, Ns_x);
|
||
|
|
parfor sp = 1:Ns_x
|
||
|
|
X_x_kkm1(:, sp) = process_Model(t_km1, t_k, X_km1(:,sp), u_km1, zeros(Vdim,1), 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
|
||
|
|
X_v_kkm1 = zeros(Xdim, Ns_v);
|
||
|
|
parfor sp = 1:Ns_v
|
||
|
|
X_v_kkm1(:, sp) = process_Model(t_km1, t_k, x_km1, u_km1, V_km1(:,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) + W_m_i * sum(X_v_kkm1, 2);
|
||
|
|
|
||
|
|
% Compute covariance of the prediction:
|
||
|
|
X_x_kkm1mx = W_c1_i * (X_x_kkm1(:,2:Xdim+1)-X_x_kkm1(:,Xdim+2:end))*(X_x_kkm1(:,2:Xdim+1)-X_x_kkm1(:,Xdim+2:end)).' + W_c1_i * (X_v_kkm1(:,1:Vdim)-X_v_kkm1(:,Vdim+1:end))*(X_v_kkm1(:,1:Vdim)-X_v_kkm1(:,Vdim+1:end)).';
|
||
|
|
X_x_kkm1px = W_c2_i * (bsxfun(@minus,X_x_kkm1(:,2:Xdim+1)+X_x_kkm1(:,Xdim+2:end), 2*X_x_kkm1(:,1)))*(bsxfun(@minus,X_x_kkm1(:,2:Xdim+1)+X_x_kkm1(:,Xdim+2:end), 2*X_x_kkm1(:,1))).' + W_c2_i * (bsxfun(@minus,X_v_kkm1(:,1:Vdim)+X_v_kkm1(:,Vdim+1:end), 2*X_x_kkm1(:,1)))*(bsxfun(@minus,X_v_kkm1(:,1:Vdim)+X_v_kkm1(:,Vdim+1:end), 2*X_x_kkm1(:,1))).';
|
||
|
|
P_kkm1 = X_x_kkm1mx + X_x_kkm1px;
|
||
|
|
|
||
|
|
|
||
|
|
%% Calculate sigma-points for measurement-update:
|
||
|
|
|
||
|
|
% Build the augmented system:
|
||
|
|
|
||
|
|
switch sigmaComputeMethod
|
||
|
|
case false
|
||
|
|
% Compute sqrt(P_kkm1) with Cholesky factorization
|
||
|
|
[S_xkkm1, chk] = chol(P_kkm1,'lower');
|
||
|
|
if chk > 0
|
||
|
|
error('CDKF: Error in Cholesky factorization at time update.')
|
||
|
|
end
|
||
|
|
h_S_xkkm1 = h * S_xkkm1;
|
||
|
|
% Compute sqrt(Rn) with Cholesky factorization
|
||
|
|
[S_n, chk] = chol(Rn,'lower');
|
||
|
|
if chk > 0
|
||
|
|
error('CDKF: Error in Cholesky factorization at time update.')
|
||
|
|
end
|
||
|
|
h_S_n = h * S_n;
|
||
|
|
case true
|
||
|
|
% Compute sqrt(P_kkm1) with SVD in case there is a 0 eigenvalue
|
||
|
|
[U_xkkm1, Sigma_xkkm1, V_xkkm1] = svd(P_kkm1,0);
|
||
|
|
h_S_xkkm1 = h * U_xkkm1 * sqrt(Sigma_xkkm1) * V_xkkm1.';
|
||
|
|
% Compute sqrt(Rn) with SVD in case there is a 0 eigenvalue
|
||
|
|
[U_n, Sigma_n, V_n] = svd(Rn,0);
|
||
|
|
h_S_n = h * U_n * sqrt(Sigma_n) * V_n.';
|
||
|
|
otherwise
|
||
|
|
error("CDKF: Please select a correct sigma point computation method !");
|
||
|
|
end
|
||
|
|
|
||
|
|
X_kkm1 = [x_kkm1, repmat(x_kkm1, 1, Xdim) + h_S_xkkm1, repmat(x_kkm1, 1, Xdim) - h_S_xkkm1];
|
||
|
|
N_km1 = [h_S_n, -h_S_n];
|
||
|
|
|
||
|
|
%% Measurement-update equations:
|
||
|
|
|
||
|
|
Y_x_kkm1 = zeros(Ydim, Ns_x);
|
||
|
|
parfor sp = 1:Ns_x
|
||
|
|
Y_x_kkm1(:, sp) = measurement_Model(X_kkm1(:,sp), zeros(Ydim,1));
|
||
|
|
end
|
||
|
|
Y_n_kkm1 = zeros(Ydim, Ns_n);
|
||
|
|
parfor sp = 1:Ns_n
|
||
|
|
Y_n_kkm1(:, sp) = measurement_Model(x_kkm1, N_km1(:,sp));
|
||
|
|
end
|
||
|
|
|
||
|
|
% Expected measurement:
|
||
|
|
y_kkm1 = W_m_0_n * Y_x_kkm1(:, 1) + W_m_i * sum(Y_x_kkm1(:, 2:end), 2) + W_m_i * sum(Y_n_kkm1, 2);
|
||
|
|
|
||
|
|
% Compute covariance of the prediction:
|
||
|
|
Y_kkm1mx = W_c1_i * (Y_x_kkm1(:,2:Xdim+1)-Y_x_kkm1(:,Xdim+2:end))*(Y_x_kkm1(:,2:Xdim+1)-Y_x_kkm1(:,Xdim+2:end)).' + W_c1_i * (Y_n_kkm1(:,1:Ydim)-Y_n_kkm1(:,Ydim+1:end))*(Y_n_kkm1(:,1:Ydim)-Y_n_kkm1(:,Ydim+1:end)).';
|
||
|
|
Y_kkm1px = W_c2_i * (bsxfun(@minus,Y_x_kkm1(:,2:Xdim+1)+Y_x_kkm1(:,Xdim+2:end), 2*Y_x_kkm1(:,1)))*(bsxfun(@minus,Y_x_kkm1(:,2:Xdim+1)+Y_x_kkm1(:,Xdim+2:end), 2*Y_x_kkm1(:,1))).' + W_c2_i * (bsxfun(@minus,Y_n_kkm1(:,1:Ydim)+Y_n_kkm1(:,Ydim+1:end), 2*Y_x_kkm1(:,1)))*(bsxfun(@minus,Y_n_kkm1(:,1:Ydim)+Y_n_kkm1(:,Ydim+1:end), 2*Y_x_kkm1(:,1))).';
|
||
|
|
P_yy = Y_kkm1mx + Y_kkm1px;
|
||
|
|
|
||
|
|
% Compute covariance of predicted observation and predicted state:
|
||
|
|
P_xy = W_m_i*h_S_xkkm1*(Y_x_kkm1(:, 2:Xdim+1)-Y_x_kkm1(:, Xdim+2:end)).';
|
||
|
|
|
||
|
|
% Compute innovation vector:
|
||
|
|
inov = y_k - y_kkm1;
|
||
|
|
|
||
|
|
% Compute the Kalman gain:
|
||
|
|
K = P_xy / P_yy;
|
||
|
|
|
||
|
|
% State correction:
|
||
|
|
x = x_kkm1 + K*inov;
|
||
|
|
|
||
|
|
% Covariance correction:
|
||
|
|
P = P_kkm1 - K * P_yy * K.';
|
||
|
|
|
||
|
|
end % cdkf
|