| 卡尔曼滤波在自由落体运动目标跟踪中的应用（含matlab代码） | 您所在的位置：网站首页 › 自由落体运动视频 › 卡尔曼滤波在自由落体运动目标跟踪中的应用（含matlab代码） |
卡尔曼滤波在自由落体运动目标跟踪中的应用（含matlab代码）
|
今天接着肝卡尔曼滤波，今天针对自由落体运动目标跟踪，由于上一篇针对温度的卡尔曼滤波是一维的测量，较为简单；所以今天的自由落体运动目标的跟踪针对二维来进行。同时还引入了控制矩阵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导航定位系统进行学习和研究。
|
卡尔曼增益：
后验估计：
，初始误差为
。
【本文地址】