卡尔曼滤波用于自由落体运动目标跟踪问题，MATLAB程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能说明：Kalman滤波用于自由落体运动目标的跟踪问题
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function main 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=1000;     %仿真时间，时间序列总数

%噪声
Q=[00;00];      %过程噪声方差为0，即下落过程忽略空气阻力
R=1;              %观测噪声方差
W=sqrt（Q）*randn（2N）;        %sqrt（）为求平方根函数；Q为0，则W=0，在此写出，方便对照理解
V=sqrt（R）*randn（1N）;        %测量噪声V（k）

%系统矩阵
A=[11;01];          %状态转移矩阵
B=[0.5;1];            %控制量
U=-1;
H=[10];              %观测矩阵

%初始化
X=zeros（2N）;        %物体真实状态
X（:1）=[95;1];       %初始位移和速度
P0=[100;01];       %初始误差
Z=zeros（1N）;       
Z（1）=H*X（:1）;       %初始观测值
Xkf=zeros（2N）;      %Kalman估计状态初始化
Xkf（:1）=X（:1）;
err_P=zeros（N2）;
err_P（11）=P0（11）;
err_P（12）=P0（22）;
I=eye（2）;           %二维状态
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N
    %物体下落，受状态方程驱动
    X（:k）=A*X（:k-1）+B*U+W（k）;
    
    %位移传感器对目标进行观测
    Z（k）=H*X（:k）+V（k）;
    
    %Kalman滤波
   X_pre=A*Xkf（:k-1）+B*U;   %状态预测
   P_pre=A*P0*A‘+Q;          %协方差预测
   Kg=P_pre*H‘*inv（H*P_pre*H‘+R）;  %计算Kalman增益