您现在的位置:首页 >> 技术文章 >> MATLAB技术 >> 内容

MATLAB代做|FPGA代做-卡尔曼滤波源码程序

时间:2021-2-11 11:34:39 点击:

  核心提示:MATLAB代做|FPGA代做-卡尔曼滤波源码程序...
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)
    0
    yna(1)
    0
    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');


联系:highspeedlogic

QQ :1224848052

微信:HuangL1121

邮箱:1224848052@qq.com

网站:http://www.mat7lab.com/

网站:http://www.hslogic.com/

微信扫一扫:

作者:卡尔曼滤波源码 来源:卡尔曼滤波源码
  • 您是如何找到本站的?
  • 百度搜索
  • Google搜索
  • 查阅资料过程中
  • 论坛发现
  • 百度贴吧发现
  • 朋友介绍
本站最新成功开发工程项目案例
相关文章
  • 没有相关文章
相关评论
发表我的评论
  • 大名:
  • 内容:
  • matlab代做|matlab专业代做|matlab淘宝代做|python人工智能代做|FPGA项目合作|C代做|C++代做(www.hslogic.com) © 2021 版权所有 All Rights Reserved.
  • Email:highspeed_logic@163.com 站长QQ: 1224848052

    专业代做/代写/承接、MATLAB、SIMULINK、FPGA项目、博士/硕士/本科毕业设计、课题设计、论文,毕业论文,Coursework、Eassy、Assignment