How to apply adaptive algorithm using k=block.CurrentTime in the s-function level2?

1 view (last 30 days)
Here is my s-function level2 algorithm for EKF
function Update(block)
Q = block.DialogPrm(2).Data; ??? this Q will be adapt later if k>M
R = block.DialogPrm(3).Data;
D = block.InputPort(2).Data;
W = block.InputPort(3).Data;
meas = block.InputPort(1).Data;
xhat = block.Dwork(1).Data;
P=reshape(block.Dwork(2).Data,4,4);
k=block.CurrentTime; ??????
%%1. Find the Jacobian
.....
%%2. Propagate the covariance matrix: (Time update)
P = A*P*A' + Q; ####to apply adaptive method for Q
%%3. Propagate the estimate:(Time update)
xhat(1) = ...
xhat(2) = ...
xhat(3) = ...
xhat(4) = ...
xhat-=[xhat(1);xhat(2);xhat(3);xhat(4)];
%%4 a). Compute observation estimates:(Measurement Update)
H = [1 0 0 0;0 1 0 0;0 0 1 0];
yhat = H*xhat;
%%4 c). Compute residual (Measurement Update)
residual = meas - yhat;
%%5. Compute Kalman Gain:(Measurement Update)
K = P*H'/(H*P*H'+ R);
%%6. Update estimate (Measurement Update)
xhat = xhat- + K*residual;
%%7. Update Covariance Matrix (Measurement Update)
P = (eye(4)-K*H)*P*(eye(4)-K*H)' + K*R*K';
block.Dwork(1).Data = xhat;
block.Dwork(2).Data = P(:);
---------------------------------------------------- Supposed i want to apply adaptive method for Q where at time k, the algorithm is defined as below:
ep(:,k)=meas(:,k)-H*xhat(:,k);
eta(:,k)=meas(:,k)-H*xhat-(:,k);
w(:,k)=K*(meas(:,k) - yhat);
Is(:,2*k)=ep(:,k);
Is(:,2*k-1)=eta(:,k);
if k>M
for i=1:4
DVQ=sum(w(i,k-M:k).^2)/(M);
end
Q=diag(DVQ)
end
------------------------------------------------- My question. how do i integrate the adaptive algorithm defined above into my s-function level2 (update)? in the s-function i have defined k=block.CurrentTime. Any advise? Thank you.

Answers (0)

Categories

Find more on Systems of Nonlinear Equations in Help Center and File Exchange

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!