82 lines
3.6 KiB
Mathematica
82 lines
3.6 KiB
Mathematica
|
function [x, S, K, inov, x_kkm1, S_kkm1, S_xy, S_yy] = srekf_opt(t_km1, t_k, x_km1, u_km1, S_km1, y_k, Sv, Sn, 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
|
||
|
|
|
||
|
|
% SREKF: Square Root Extended Kalman Filter
|
||
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
% 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
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
% 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
|
||
|
|
% S_xy: predicted state and observation covariance
|
||
|
|
% S_yy: matrix square root of inovation covariance
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
|
||
|
|
% Numerical Jacobian options:
|
||
|
|
jacobianOptions.epsVal = 1e-7;
|
||
|
|
|
||
|
|
% Problem dimensions:
|
||
|
|
% Xdim = length(x_km1); % Number of states
|
||
|
|
Vdim = size(Sv, 1); % Number of noise states
|
||
|
|
Ydim = size(Sn, 1); % Number of observations
|
||
|
|
|
||
|
|
% Expected prediction and measurement:
|
||
|
|
x_kkm1 = process_Model(t_km1, t_k, x_km1, 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);
|
||
|
|
y_kkm1 = measurement_Model(x_kkm1,zeros(Ydim,1));
|
||
|
|
|
||
|
|
% Compute the Jacobian using the state at k-1:
|
||
|
|
hF = @(X) process_Model(t_km1, t_k, X, 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);
|
||
|
|
hG = @(V) process_Model(t_km1, t_k, x_km1, u_km1, V, 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);
|
||
|
|
hH = @(X) measurement_Model(X,zeros(Ydim,1));
|
||
|
|
hD = @(Y) measurement_Model(x_kkm1,Y);
|
||
|
|
F = computeNumJacobian(x_km1, hF, jacobianOptions);
|
||
|
|
G = computeNumJacobian(zeros(Vdim,1), hG, jacobianOptions);
|
||
|
|
H = computeNumJacobian(x_kkm1, hH, jacobianOptions);
|
||
|
|
D = computeNumJacobian(zeros(Ydim,1), hD, jacobianOptions);
|
||
|
|
|
||
|
|
% Compute covariance of the prediction:
|
||
|
|
[~,S_kkm1] = qr([S_km1.'*F.'; Sv.'*G.'],0);
|
||
|
|
S_kkm1 = S_kkm1.';
|
||
|
|
|
||
|
|
[~,R] = qr([Sn.'*D.' zeros(Ydim, Vdim); S_kkm1.'*H.' S_kkm1.'],0);
|
||
|
|
|
||
|
|
% Compute covariance of predicted observation:
|
||
|
|
S_yy = R(1:Ydim,1:Ydim).';
|
||
|
|
|
||
|
|
% Compute covariance of predicted observation and predicted state:
|
||
|
|
S_xy = R(1:Ydim,Ydim+1:end).';
|
||
|
|
|
||
|
|
% Compute innovation vector:
|
||
|
|
inov = y_k - y_kkm1;
|
||
|
|
|
||
|
|
% Kalman gain:
|
||
|
|
K = S_xy/S_yy;
|
||
|
|
|
||
|
|
% State correction:
|
||
|
|
x = x_kkm1 + K*inov;
|
||
|
|
|
||
|
|
% Covariance correction:
|
||
|
|
S = R(Ydim+1:end,Ydim+1:end).';
|
||
|
|
|
||
|
|
end % srekf
|