搜索
bottom↓
回复: 1

卡尔曼matleb程序请教

[复制链接]
(138846602)

出0入0汤圆

发表于 2016-7-1 16:23:56 | 显示全部楼层 |阅读模式
相信大家对卡尔曼这个名字并不陌生,网上也有很多代码,我初次接触也对网上的代码(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');

论坛公益广告: VIP+与VIP++福利 >>

(138846560)

出0入0汤圆

 楼主| 发表于 2016-7-1 16:24:38 | 显示全部楼层
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;
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子论坛 ( 公安交互式论坛备案:44190002001997 粤ICP备09047143号-1 )

GMT+8, 2020-11-24 16:53

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表