kal.m

kalman滤波学习的Maltab程序

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