Need help with debugging on kalman filter
Show older comments
Hi all,
I need to modify these codes below on Kalman filter, but there seems to be some error.
Can anyone help please?
clear N=3; %number of samples
Nrun=100; %number of runs
T=1; %sampling interval
sw=1; % sigma_w
sv=1; % sigma_v
A=[1 T; 0 1]; %process matrix
C=[1 0]; %process matrix
R1=sw^2*[T^4/4 T^3/2; T^3/2 T^2]; %covariance of process noise
R2=sv^2; %covariance of measurement noise
Pm(:,:,1)=1e5*eye(2); %P(0|-1)
for k=1:N
Kf(:,k)=Pm(:,:,k)*C’*(C*Pm(:,:,k)*C’+R2)^(-1); %Kf(:,k)=Kf(k)
K(:,k)=(A*Pm(:,:,k)*C’)*(C*Pm(:,:,k)*C’+R2)^(-1); %K(:,k)=K(k)
P(:,:,k)=Pm(:,:,k)-(Pm(:,:,k)*C’)*(C*Pm(:,:,k)*C’+R2)^(-1)*C*Pm(:,:,k); %P(:,:,k)=P(N|N) Pm(:,:,k+1)=A*Pm(:,:,k)*A’+R1-K(:,k)*(C*Pm(:,:,k)*C’+R2)*K(:,k)’; %Pm(:,:,k+1)=P(N+1|N)
end
2 Comments
Geoff Hayes
on 9 Nov 2020
Wei - what modifications do you need to make? What is the error?
Wei Jun Chan
on 10 Nov 2020
Answers (0)
Categories
Find more on State Estimation 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!