Need help with debugging on kalman filter

Wei Jun Chan

2020 11 9

0 个回答

7 次查看（30 天）

0 个投票

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 