卡尔曼滤波源码程序 - matlab算法设计

下载频道> 资源分类> matlab源码> 算法设计> 卡尔曼滤波源码程序

标题：卡尔曼滤波源码程序
分享到：

所属分类: 算法设计资源类型：文件大小: 36.23 KB 上传时间: 2016-01-30 12:44:27 下载次数: 10 资源积分：1分提供者: 源码共享卡尔曼滤波源码程序

内容：
卡尔曼滤波源码程序，程序员在编程的过程中可以参考学习使用，希望对IT程序员有用，此源码程序简单易懂、方便阅读，有很好的学习价值！

clc;

clear;

close all;

al=40*pi/180; % angle of velocity at the bigining

v0=10; % initial velocity

g=9.8; % gravitation constant

vx=v0*cos(al);

vy=v0*sin(al); % initial velocities

x=0;

y=0; % initial position

dt=0.02; % time step

sgx=0.02; % sigma_x - nose lavel for x

sgy=0.02; % sigma_y - nose lavel for y

% generate original trajectory:

xa=[]; % here x coordinaties will be stored

ya=[]; % here y coordinaties will be stored

vxa=[]; % here x-component of velocity will be stored

vya=[]; % here y-component of velocity will be stored

ta=[]; % here times will be stored

xna=[]; % here noised x coordinaties will be stored

yna=[]; % here noised y coordinaties will be stored

t=0; % start time

while 1 % infinite loop, break used

xa=[xa x];

ya=[ya y];

vxa=[vxa vx];

vya=[vya vy];

ta=[ta t];

xna=[xna x+sgx*randn];

yna=[yna y+sgy*randn]; % add gaussian noise

if y<0 % when return to ground

break % stop motion

end

% update values

x=x+vx*dt; % vx~(x(k+1)-x(k))/dt => x(k+1)~x(k)+vx*dt

y=y+vy*dt;

vy=vy-g*dt;

t=t+dt;

% note: vx not change

end

% plot nosied trajectory

%plot(xa,ya,'b-',xna,yna,'r--');

figure;

plot(xna,yna,'b-');

xlabel('x, m');

ylabel('y, m');

%legend('real curve','noised curve');

% apply kalman filtering:

R=[sgx^2 0

0 sgy^2]; % covariance matrix for nose in x and y

H=[1 0 0 0 0;

0 0 1 0 0]; % observarion matrix

F=[ 1 dt 0 0 0

0 1 0 0 0

0 0 1 dt 0

0 0 0 1 dt

0 0 0 0 1 ]; % state transition matrix

P=eye(5); % inital gues for P-matrix

I=eye(5); % unit matrix

% estimated state vector form:

% xc =[ x

% vx

% y

% vy

% ay ]

xc=[xna(1)

yna(1)

0]; % guess for initial value of estimation of state vector

% for x y get initial noised position, for vx vy ay is zeros

z=[xna

yna]; % observation

xca=xc; % will be stored in xca

nc=2; % loop counter, stert from second step

for t=ta(2:end)

Ps=F*P*F';

K=Ps*H'*(H*Ps*H'+R)^(-1);

xc=F*xc+K*(z(:,nc)-H*F*xc);

P=(I-K*H)*Ps*(I-K*H)'+K*R*K';

%P=(I-K*H)*Ps;

xca=[xca xc]; % store

nc=nc+1;

end

% add to plot filtered tragectory:

hold on;

plot(xca(1,:),xca(3,:),'r-','linewidth',2);

legend('noised','filtered');

title('trajectory');

% plot velocities graph in separated window:

figure;

plot(ta,xca(2,:),'r-','linewidth',2);

hold on;

plot(ta,vxa,'r--');

plot(ta,xca(4,:),'b-','linewidth',2);

plot(ta,vya,'b--');

xlabel('time, s');

legend('filtered vx','original vx','filtered vy','original vy');

title('velocities');

文件列表（点击上边下载按钮，如果是垃圾文件请在下面评价差评或者投诉）:

卡尔曼滤波源码程序/
卡尔曼滤波源码程序/kalman_filter/
卡尔曼滤波源码程序/kalman_filter/description.docx
卡尔曼滤波源码程序/kalman_filter/kalman_filter.m

关键词：卡尔曼 源码 程序

matlab源码下载排行

回到顶部

友情链接

matlab源码网站源码编程语言网络编程手机通讯编程 KUNAG 官网 KUNAG 电商更多

: 联系方式| 版权声明| 招聘信息| 广告服务| 银行汇款| 法律顾问| 兼职技术| 付款方式| 关于我们|; 网站客服程序员兼职招聘 411241577@qq.com; 沪ICP备19040327号-3; 公安备案号：沪公网安备 31011802003874号; 库纳格流体控制系统（上海）有限公司版权所有; Copyright © 1999-2014, GUSUCODE.COM, All Rights Reserved

关键词： 卡尔曼 源码 程序

相关推荐

matlab源码下载排行

友情链接

关键词：卡尔曼源码程序