www.gusucode.com > 基于matlab编程雷达目标航迹跟踪源码程序 > 基于matlab编程雷达目标航迹跟踪源码程序/数据处理程序/kalman_filter_initial.m

    function [state_initial,variance_initial] = kalman_filter_initial(first_point_of_track ,second_point_of_track ,ts ,...
    sigma_r,sigma_a,sigma_e)
% 函数实现功能：
% kalman滤波初始化程序，为后面的递推算法的实现，产生必需的初始化量
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 输入变量 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% first_point_of_track --> 本航迹的第一个点,3*1的矩阵,第一行表示距离,第二行表示方位角，第三行表示俯仰角
% second_point_of_track --> 本航迹的第二个点
% ts --> 每两批数据之间的间隔时间
% sigma_r --> 距离观测噪声标准差
% sigma_a --> 方位角观测噪声标准差
% sigma_e --> 俯仰角观测噪声标准差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 输入变量 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%% 输出变量 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% state_initial --> 初始状态
% variance_initial --> 初始协方差
%%%%%%%%%%%%%%%%%%%%%%%%%%%% 输出变量 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求初始状态%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
range_observe=[first_point_of_track(1),second_point_of_track(1)];
azimuth_observe=[first_point_of_track(2),second_point_of_track(2)];
elevation_observe=[first_point_of_track(3),second_point_of_track(3)];
Vr(2)=(range_observe(2)-range_observe(1))/ts;
Va(2)=(azimuth_observe(2)-azimuth_observe(1))/ts;
Ve(2)=(elevation_observe(2)-elevation_observe(1))/ts;
state_initial=[range_observe(2) Vr(2) azimuth_observe(2) Va(2) elevation_observe(2) Ve(2)]';%初始状态
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求初始状态%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求初始协方差%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
variance_initial=[sigma_r^2 sigma_r^2/ts 0 0 0 0;sigma_r^2/ts 2*sigma_r^2/ts^2 0 0 0 0;0 0 sigma_a^2 sigma_a^2/ts 0 0;...
    0 0 sigma_a^2/ts 2*sigma_a^2/ts^2 0 0;0 0 0 0 sigma_e^2 sigma_e^2/ts;0 0 0 0 sigma_e^2/ts 2*sigma_e^2/ts^2];%初始协方差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求初始协方差%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%