80 lines
3.4 KiB
Mathematica
80 lines
3.4 KiB
Mathematica
|
function [x, P, K, inov, x_kkm1, P_kkm1, P_xy, P_yy] = ekf_opt(t_km1, t_k, x_km1, u_km1, P_km1, y_k, Rv, Rn, 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
|
||
|
|
|
||
|
|
% EKF: Extended Kalman Filter
|
||
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
% 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
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
% 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
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
|
||
|
|
% Numerical Jacobian options:
|
||
|
|
jacobianOptions.epsVal = 1e-7;
|
||
|
|
|
||
|
|
% Problem dimensions:
|
||
|
|
% Xdim = length(x_km1); % Number of states
|
||
|
|
Vdim = size(Rv, 1); % Number of noise states
|
||
|
|
Ydim = size(Rn, 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 innovation vector:
|
||
|
|
inov = y_k - y_kkm1;
|
||
|
|
|
||
|
|
% Compute covariance of the prediction:
|
||
|
|
P_kkm1 = F*P_km1*F.' + G*Rv*G.';
|
||
|
|
|
||
|
|
% Compute covariance of predicted observation and predicted state:
|
||
|
|
P_xy = P_kkm1*H.';
|
||
|
|
|
||
|
|
% Compute covariance of predicted observation:
|
||
|
|
P_yy = H*P_kkm1*H.' + D*Rn*D.';
|
||
|
|
|
||
|
|
% Kalman gain:
|
||
|
|
K = P_xy / P_yy;
|
||
|
|
|
||
|
|
% State correction:
|
||
|
|
x = x_kkm1 + K*inov;
|
||
|
|
|
||
|
|
% Covariance correction:
|
||
|
|
% P = P_kkm1 - K*P_yy*K.';
|
||
|
|
P = P_kkm1 - K*H*P_kkm1;
|
||
|
|
|
||
|
|
end % ekf
|