今天接着肝卡尔曼滤波，今天针对自由落体运动目标跟踪，由于上一篇针对温度的卡尔曼滤波是一维的测量，较为简单；所以今天的自由落体运动目标的跟踪针对二维来进行。同时还引入了控制矩阵B和控制量U。
首先还是先预习一下卡尔曼的知识。
参考内容：书籍《卡尔曼滤波原理及应用------matlab仿真》
卡尔曼知识
模型建立
观测方程：Z(k)=H*X(k)+V(k）；
状态方程：X(k)=A*X(k-1)+W(k-1);
其中，X（k）为系统在时刻k的状态，Z（k）为对应状态的测量值。W（k）为输入的白噪声（也是过程误差），V（k）为观测噪声（也是测量误差），W（k），V（k）是均值为零，方差阵各为Q和R的不相关白噪声。A为状态转移矩阵，H为观测矩阵。
卡尔曼滤波：
预测 校正
先验估计 ：
卡尔曼增益：
先验协方差误差 ：
后验估计：
协方差:
样例分析：
某一物体在重力场做自由落体运动，有一观测装置在对物体的位移进行检测，传感器会受到位置的独立分布随机信号的干扰。我们需要估计该物体的运动位移和速度两个变量。
对于上述这个二阶系统，我们已知的是，该物体的加速度为g,设物体的位移x1和速度x2，定义如下的向量
现在推导该自由落体目标的状态转移矩阵。由运动方程，可得到运动物体的状态方程。
给定位置观测装置，在测量值受到某种独立，随机干扰v(k)的影响时，观测方程可写为：
给定v(k)的方差R（k）=1；物体初始状态
，初始误差为
。
并且由于运动方程的物理模型可知：
(个人理解由于是下落过程中忽略了空气阻力).
在上述条件具备的情况下，可以进行下一步的kalman滤波的递推过程。
其中
另外的递推关系式为：
此外第k+1时刻的预测值可以写为：
按照上述算法流程，对自由落体运动目标进行kalman滤波和预测。
关于自由落体的matlab具体算法如下所示：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%kalman滤波用于自由落体运动目标的跟踪问题
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear all
close all
N=1000; %仿真时间，时间序列号的总数
%噪声
Q=[0 0;0 0]; %过程噪声方差为0，即下落过程中忽略了空气阻力。
R=[1,0;0,1]; %观测噪声方差
W=sqrt(Q)*randn(2,N); %过程噪声
V=sqrt(R)*randn(2,N); %测量噪声
%系统矩阵
A=[1,1;0,1]; %状态转移矩阵
B=[0.5;1]; %控制矩阵
U=1; %控制量，相当于g，此处为了便于计算令其等于1
C=[1,0;0,1]; %观测矩阵，相当于公式中的H
%参数初始化
X=zeros(2,N); %物体真实状态初始化
X(:,1)=[95;1]; %物体真实状态初始位置和速度
P0=[10,0;0,1]; %协方差误差初始化
Z=zeros(2,N); %测量初始化
Z(:,1)=C*X(:,1)+V(:,1); %测量状态初始位置
Xkf=zeros(2,N); %kalman滤波估计状态初始化
Xkf(:,1)=X(:,1); %kalman滤波估计状态初始位置
err_P=zeros(N,2); %误差均方值初始化
err_P(1,1)=P0(1,1);
err_P(1,2)=P0(2,2);
I=eye(2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N
%物体下落，受状态方程的驱动
X(:,k)=A*X(:,k-1)+B*U+W(:,k);
%测量
Z(:,k)=C*X(:,k)+V(:,k);
%kalman滤波
X_pre=A*Xkf(:,k-1)+B*U;
P_pre=A*P0*A'+Q;
K=P_pre*C'/(C*P_pre*C'+R);
Xkf(:,k)= X_pre+K*(Z(:,k)-C*X_pre);
P0=(I-K*C)*P_pre;
%误差均方差
err_P(k,1)=P0(1,1);
err_P(k,2)=P0(2,2);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%误差计算
meassure_err_x=zeros(1,N); %测量值与真实值的位移误差
meassure_err_v=zeros(1,N); %测量值与真实值的速度误差
kalman_err_x=zeros(1,N); %kalman估计的位移与真实位移之间的偏差
kalman_err_v=zeros(1,N); %kalman估计的速度与真实速度之间的偏差
for k=1:N
meassure_err_x(k)=Z(1,k)-X(1,k); %测量值与真实值的误差
meassure_err_v(k)=Z(2,k)-X(2,k); %测量值与真实值的速度误差
kalman_err_x(k)=Xkf(1,k)-X(1,k); %kalman估计的位移与真实位移之间的偏差
kalman_err_v(k)=Xkf(2,k)-X(2,k); %kalman估计的速度与真实速度之间的偏差
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%画图输出
%噪声图
figure
plot(W);
xlabel('采样时间/s');
ylabel('过程噪声');
title("过程噪声噪声图");
figure
plot(V);
xlabel('采样时间/s');
ylabel('测量噪声');
title("测量噪声噪声图");
%位置偏差
figure
hold on,box on,grid on;
plot(meassure_err_x,'-r*'); %测量的位移误差
plot(kalman_err_x,'-gs'); %kalman估计位置误差
legend('测量误差','kalman滤波误差');
xlabel('采样时间/s');
ylabel('位置偏差/m');
title("位置的偏差");
figure;
plot(meassure_err_v,'-r*');hold on; %测量的速度误差
plot(kalman_err_v,'-go');
legend('测量误差','kalman滤波误差');
grid on
xlabel('采样时间/s');
ylabel('速度偏差/m');
title("速度的偏差");
%误差均方值
figure
plot(err_P(:,1),'-r');
grid on
xlabel('采样时间/s');
ylabel('位移误差均方值');
title("位移误差均方值");
figure
plot(err_P(:,2),'-b');
grid on;
xlabel('采样时间/s');
ylabel('速度误差均方值');
title("速度误差均方值");
从位置和速度的估计来看，测量值受到测量噪声的影响，但kalman滤波算法则可以很好的降低噪声的干扰，在经过少数迭代后误差很快的收敛。
从速度的误差均方值和位移的误差均方值来看，可见kalman滤波对高斯噪声的处理非常有效。
总结：
关于kalman滤波在二维情况下的工作情况，就先这样，如果有那些错的地方还请指正，下次会针对舰船的GPS导航定位系统进行学习和研究。
