www.gusucode.com > 基于机动目标跟踪课题的整个算法matlab程序 > Karlman.m
function [S,M]=Karlman(S_forward,M_forward,X,A,Q,C,H) %卡尔曼滤波 %参数说明 % X--观测数据矢量 % A--状态矩阵 % Q--驱动噪声协方差 % C--观测噪声协方差 % H--观测方程句柄 % S--输出数据矢量 % S_foward--前次输出矢量 % M_forward--前次预测矩阵 global T; %预测 S=feval(A,S_forward); %状态转换矩阵 % A=[1,0,-S(4)/2*sin(S(3)/2)*T,cos(S(3)/2)*T,0;... % 0,1,-S(4)/2*cos(S(3)/2)*T,-sin(S(3)/2)*T,0;... % 0,0,1,-S(5)*T/(S(4))^2,T/S(4);... % 0,0,0,1,0;... % 0,0,0,0,1]; A=[1,0,-S(4)/2*sin(S(3)/2)*T,cos(S(3)/2)*T,0;... 0,1,-S(4)/2*cos(S(3)/2)*T,-sin(S(3)/2)*T,0;... 0,0,1,-S(5)*T/(S(4))^2,T/S(4);... 0,0,0,1,0;... 0,0,0,0,1]; %最小预测MSE矩阵 M=M_forward; M=A*M*A'+Q; %卡尔曼增益矩阵 K=M*H'*inv(C+H*M*H'); %修正 S=S+K*(X-H*S); %最小MSE矩阵 M=M-K*H*M;