Kalman滤波在自由落体运动目标追踪中的应用

这是Kalman滤波的一个简单应用实例，实现了对自由落体物体的位移和速度的估计。代码简单，每一行都添加注释，方便大家的理解。

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

%噪声
Q=[00;00];%过程方差为0，即下落过程忽略空气阻力
R=1;%观测噪声方差
W=sqrt（Q）*randn（2N）;%既然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

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2227  2016-09-07 11:24  main.m

----------- ---------  ---------- -----  ----

                 2227                    1