您现在的位置:首页 >> ★免费资源 >> 源码下载 >> 内容

matlab代做highspeedlogic★去偏卡尔曼滤波

时间:2015-10-4 19:01:32 点击:

  核心提示:matlab代做,FPGA代做,淘宝,专业代做MATLAB、FPGA博士/硕士/本科毕业设计、项目仿真、Coursework、Assignment ...

matlab代做FPGA代做淘宝专业代做MATLABFPGA博士/硕士/本科毕业设计项目仿真CourseworkAssignment

QQ: 1224848052

function [filter_data,k1,k2]=data_kalman_filter(view_data,point_view_data,N,object_init_state,Q1,point_Q2,T,l)
%定义初试滤波
filter_data0=object_init_state';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ch=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];               %定义状态转移矩阵

h=[1 0 0 0;0 0 1 0];                               %观测转移矩阵
%观测误差是随时间变化的所以需计算观测误差
cos_2_v_thita=(object_init_state(1,1)^2)/(object_init_state(1,1)^2+object_init_state(1,3)^2);
sin_2_v_thita=1-cos_2_v_thita;
R_2=(object_init_state(1,1)^2+object_init_state(1,3)^2);
Q2=zeros(2,2);       
Q2(1,1)=point_Q2(1,1)*cos_2_v_thita+R_2*sin_2_v_thita*point_Q2(2,2);
%Q2(1,2)=sqrt(cos_2_v_thita)*sqrt(sin_2_v_thita)*(point_Q2(1,1)-R_2*point_Q2(2,2));
Q2(1,2)=0;
Q2(2,1)=Q2(1,2);
Q2(2,2)=point_Q2(1,1)*sin_2_v_thita+R_2*cos_2_v_thita*point_Q2(2,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
pre_data(:,1)=ch*filter_data0;
p_init=100*eye(4);                                                   %初始协方差定义 
p_old=p_init;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%根据初始预测计算滤波值
%通过输入观测值进行卡尔曼滤波
for i=1:N
    [filter_data(:,i),pre_data(:,i+1),p_new,k11,k21,Q2]=kalman_filter(pre_data(:,i),view_data(:,i),point_view_data(:,i),p_old,ch,Q1,Q2,h,l,point_Q2);
    p_old=p_new;
    k1(:,i)=k11;
    k2(:,i)=k21;
end
 

%由各次的误差计算出目标的平均误差,作为跟踪的均值估计
function [ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y]=err_count(ME_temp_view_err_x,ME_temp_filter_err_x,ME_temp_view_err_y,ME_temp_filter_err_y);
[ROW,COL]=size(ME_temp_view_err_x);
for i=1:COL
    ME_final_view_err_x(i)=sqrt(ROW)*sum(ME_temp_view_err_x(:,i))/ROW;
    ME_final_filter_err_x(i)=sqrt(ROW)*sum(ME_temp_filter_err_x(:,i))/ROW;
    ME_final_view_err_y(i)=sqrt(ROW)*sum(ME_temp_view_err_y(:,i))/ROW;
    ME_final_filter_err_y(i)=sqrt(ROW)*sum(ME_temp_filter_err_y(:,i))/ROW;
end
 

function [final_filt_k1,final_filt_k2]=filt_count(filt_k1,filt_k2,N1)
sum_filt_k1=zeros(4,200);
sum_filt_k2=zeros(4,200);
for i=1:N1
    for j=1:200
       A(j,1:4)=filt_k1(i,:,j);
       B(j,1:4)=filt_k2(i,:,j);
   end
   A=A';
   B=B';
   sum_filt_k1=sum_filt_k1+A;
   sum_filt_k2=sum_filt_k2+B;
   A=zeros(200,4);
   B=zeros(200,4);
end
final_filt_k1=sum_filt_k1/N1;
final_filt_k2=sum_filt_k2/N1;
    
function [x_filt,x_pre,p_new,k11,k21,Q2]=kalman_filter(pre_data,z,point_z,p_old,ch,Q1,Q2,h,l,point_Q2)
%x_prev_new=ch*pre_filt_data;
e=exp(-point_Q2(2,2))-exp(-point_Q2(2,2)/2);
r=point_z(1);
thita=point_z(2);
z1=z-[r*cos(thita)*e;r*sin(thita)*e];
pre_p=ch*p_old*(ch)'+l*Q1*(l)';
k=pre_p*(h)'*inv(h*pre_p*(h)'+Q2);
k11=k(1:4,1);
k21=k(1:4,2);
x_filt=pre_data+k*(z1-h*pre_data);
x_pre=ch*x_filt;
p_new=(eye(4)-k*h)*pre_p;
cos_2_v=x_filt(1,1)^2/(x_filt(1,1)^2+x_filt(3,1)^2);
sin_2_v=1-cos_2_v;
R_2=x_filt(1,1)^2+x_filt(3,1)^2;
Q2(1,1)=point_Q2(1,1)*cos_2_v+R_2*sin_2_v*point_Q2(2,2);
%Q2(1,2)=sqrt(cos_2_v)*sqrt(sin_2_v)*(point_Q2(1,1)-R_2*point_Q2(2,2));
Q2(1,2)=0;
Q2(2,1)=Q2(1,2);
Q2(2,2)=point_Q2(1,1)*sin_2_v+R_2*cos_2_v*point_Q2(2,2);   

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

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