卡尔曼matleb程序请教

陶新成

相信大家对卡尔曼这个名字并不陌生，网上也有很多代码，我初次接触也对网上的代码（matleb和C）进行了测试，都或多或少有些疑问
下面是我在网上找到的两段matleb代码，仿真过是可以使用的，现在的问题是代码里由库函数产生的随机数如何转换成调用我电脑里做好
的TXT文件里的数据，请高手指点！
代码一：
clear
clc;
N=300;
CON=5;
x=zeros(1,N);
x(1)=1;p=10;
Q=randn(1,N)*0.2;%过程噪声协方差 
R=randn(1,N);%观测噪声协方差 
y= R+CON;%加过程噪声的状态输出 
for k =2:N
Q1=cov(Q(1:k-1));%过程噪声协方差     
Q2=cov(R(1:k-1));
x(k)=x(k-1);%预估计k时刻状态变量的值    
p=p+Q1;%对应于预估值的协方差    
kg=p/(p+Q2);%kalman gain    
x(k)=x(k)+kg*(y(k)-x(k));
p=(1-kg)*p;
end
Q=randn(1,N)*0.2;%过程噪声协方差 
R=randn(1,N);%观测噪声协方差 
y=R+CON;%加过程噪声的状态输出 
for k=2:N
Q1=cov(Q(1:k-1));%过程噪声协方差    
Q2=cov(R(1:k-1));
x(k)=x(k-1);%预估计k时刻状态变量的值     
p=p+Q1;%对应于预估值的协方差    
kg=p/(p+Q2);%kalman gain     
x(k)=x(k)+kg*(y(k)-x(k));
p=(1-kg)*p;
end
Filter_Wid=10;
smooth_res=zeros(1,N);
kalman_p=zeros(1,N);
for i=Filter_Wid+1:N
    tempsum=0;
    kalman_m=0;
for j=i-Filter_Wid:i-1
    tempsum=tempsum+y(j);
    kalman_m=kalman_m+x(j);end
kalman_p(i)=kalman_m/Filter_Wid;
smooth_res(i)=tempsum/Filter_Wid;%平滑滤波 
end
% figure(1); % hist(y); 
t=1:N;
figure(1);
expValue=zeros(1,N);
for i=1:N
expValue(i)=CON;
end
plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k',t,kalman_p,'m');
legend('truth','estimate','measure','smooth result','smooth kalman');
%axis([0 N 0 30]) 
xlabel('Sample time');
ylabel('Room Temperature');
title('Smooth filter VS kalman filter');

陶新成

clear all;
close all;
clc;
n=40;

%point=load('point.mat');
point=load('new.txt');
%current_point=point.point;
current_point=new();

%plot(current_point(:,1),current_point(:,2),'r');

%系统方程：x(k+1)=fi*x(k)+gm*w(k)
%观测方程：z(k)=h*x(k)+v(k)

x=current_point(:,1);
y=current_point(:,2);

fi=1;
h=1;
gm=5;

w=randn(1,n);
v=randn(1,n);

xy_x(1)=x(1);
xy_y(1)=y(1);

p(1)=0;
z_x(1)=x(1)+w(1);
z_y(1)=y(1)+w(1);
R=(std(v)).^2;
Q=(std(w)).^2;
k(1)=fi*p(1)*h'*inv(h*p(1)*h'+R);
pp(1)=fi*p(1)*fi'+gm*Q*gm';

for i=2:n
    xy_x(i)=fi*xy_x(i-1)+k(i-1)*(z_x(i-1)-xy_x(i-1));
    xy_y(i)=fi*xy_y(i-1)+k(i-1)*(z_y(i-1)-xy_y(i-1));
   
    k(i)=pp(i-1)*h'*inv((h*pp(i-1)*h'+R));
    pp(i)=fi*p(i-1)*fi'+gm*Q*gm';
    p(i)=pp(i-1)-k(i)*h*p(i-1);

    z_x(i)=x(i)+w(i);
    z_y(i)=y(i)+w(i);   
end

hold on;
plot(xy_x,xy_y);
grid on;