1 / 5

EKF Matlab Equations

EKF Matlab Equations. , then. Let:. Problem Setup. ,. State Update. A(:,:,i)=linearize(qk_km1(:,i));. =. F(:,i)=[qk_km1(2,i) (-2*qk_km1(4,i)*qk_km1(3,i)*qk_km1(2,i)-(qk_km1(1,i)*qk_km1(3,i)^2)+ARF(i)) 0 0]';. qk_km1(:,i) = M(:,:,i)*F(:,i)*intdt + qk_km1(:,i);.

tuan
Download Presentation

EKF Matlab Equations

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. EKF Matlab Equations

  2. , then Let: Problem Setup ,

  3. State Update A(:,:,i)=linearize(qk_km1(:,i)); = F(:,i)=[qk_km1(2,i) (-2*qk_km1(4,i)*qk_km1(3,i)*qk_km1(2,i)-(qk_km1(1,i)*qk_km1(3,i)^2)+ARF(i)) 0 0]';

  4. qk_km1(:,i) = M(:,:,i)*F(:,i)*intdt + qk_km1(:,i); P_dot = A(:,:,i)*Pk_k(:,:,i) + Pk_k(:,:,i)*A(:,:,i)' + Q; Pk_k(:,:,i) = Pk_k(:,:,i) + (intdt)*M(:,:,i)*P_dot*M(:,:,i)'; State Update, cont. Taylor-Heun Approximation: (Mazzoni 2007, courtesy of Muhel & Vivek) M(:,:,i) = inv(eye(4) - A(:,:,i)*intdt/(2));

  5. K(:,i) = Pk_k(:,:,i)*H'*(1/(Pk_k(1,1,i)+R)); qk_k(:,i) = qk_km1(:,i) + K(:,i)*(ytrue(i)-qk_km1(1,i)); Pk_k(:,:,i) = (eye(4) - K(:,i)*H)*Pk_k(:,:,i); Measurement Update

More Related