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-Space Control Design and 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!