clear 
N=50; 
x(:,1)=[-0.5;0;0.5];%定義初始參數
y(:,1)=x(:,1);
for i=2:N
y(:,i)=y(:,1);
end
R=input('請輸入測量噪聲方差R的值: ');
Q=input('請輸入過程噪聲方差Q的值: '); 
A=[1 0 0;0 1 0;0 0 1]; 
H=A; 
R=R*A;
Q=Q*A;
p=ones(3); 
s(:,1)=[0;0;0];

for k=2:N; %卡爾曼濾波器主程序
x(:,k)=A*x(:,1)+sqrt(Q)*randn(3,1);%產生輸入信號
Z(:,k)=H*x(:,k)+ sqrt(R)*randn(3,1); %觀測值
p1=p+Q; %時間更新方程中的先驗估計誤差的協方差
K=p1/(p1+R); %測量更新方程中的增益矩陣
s(:,k)=s(:,k-1)+K*(Z(:,k)-s(:,k-1)); %狀態估計
p=p1-K*p1; %后驗估計誤差的協方差
end 
p(1,1)

for j=1:3
subplot(3,1,j);
t=1:N-1; 
plot(t-1,s(j,t+1),t-1,Z(j,t+1),'+',t-1,y(j,t+1),'-.');
legend('濾波器的估計結果','觀測值','待估常數');
xlabel('Iteration');
ylabel('Voltage');
end