Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- xp=position_data(:,1);
- yp=position_data(:,1)(:,2);
- N=length(xp);
- Q=eye(4);
- motion=zeros(4,1);
- H=[1 0 0 0
- 0 1 0 0];
- F=[1 0 1 0
- 0 1 0 1
- 0 0 1 0
- 0 0 0 1];
- x=zeros(4,1);
- P = eye(4)*1000; %initial uncertainty
- observed_x=xp+0.05*rand(N,1).*xp;
- observed_y=yp+0.05*rand(N,1).*yp;
- R=0.01^2;
- pos=[observed_x,observed_y];
- start=0;
- jj=zeros(N,2); %%jj will be the final result
- for k=start+1:length(observed_x)
- measurement=pos(k,:);
- y = measurement' - H * x;
- S = H * P * H' + R; % residual convariance
- K = P * H' * inv(S); % Kalman gain
- x = x + K*y;
- I = eye(4); % identity matrix
- P = (I - K*H)*P;
- % predict x and P
- x = F*x + motion;
- P = F*P*F' + Q;
- jj(k,:)=x(1:2);
- end
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement