卡尔曼滤波器实例:跟踪自由落体运动:设计与Matlab实现
[首发:cnblogs 作者:byeyear Email:byeyear@hotmail.com]
本文所用实例来自于以下书籍:
Fundamentals of Kalman Filtering: A Practical Approach, 3rd Edition.Paul Zarchan, Howard Musoff.
某物体位于距地面400000 ft的高空,初速度为6000 ft/s,重力加速度为32.2 ft/s2。地面雷达位于其正下方测量该物体高度,测量周期0.1s,维持30s。已知雷达测量结果的标准差为1000 ft。
嗯,原书例子所用单位就是ft。与国标折算比例为0.3048。
取地面雷达位置为坐标原点,往上为正向。物理方程如下:
$x=400000-6000t-\frac{1}{2}gt^2$
从物理学的角度,选取距离、速度、加速度这三者作为系统状态是最直观的。将上式看做关于$t$的二阶多项式,其相应的状态转换矩阵和观测模型为:
$\boldsymbol{\Phi}_k = \left[ \begin{matrix} 1&T_s&0.5T_s^2 \\ 0&1&T_s \\ 0&0&1 \end{matrix} \right]$
$\mathbf{H} = \left[ \begin{matrix} 1&0&0 \end{matrix} \right]$
初始状态向量估计设为0,状态估计误差方差为$\infty$。
matlab程序如下:
order=3; % polynomial order is 3
ts=.1; % sample period
f2m=0.3048; % feet -> meter
t=[0:ts:30-ts];
s_init=400000*f2m; % init distance
v_init=-6000*f2m; % init speed
g_init=-9.8; % gravity
s=s_init + v_init .* t + 0.5 * g_init .* t .* t;
v=v_init + g_init .* t;
g=g_init * ones(1,300);
r=(1000*f2m)^2; % noise var
n=wgn(1, 300, r, 'linear');
sn=s+n; % signal with noise
x=zeros(3, 1); % init state vector
p=99999999999999 * eye(3,3); % estimate covariance
idn=eye(3); % unit matrix
phi=[1 ts 0.5*ts*ts; 0 1 ts; 0 0 1]; % fundmental matrix (p164)
h=[1 0 0];
phis=0; % no process noise
q=phis * [ts^5/20 ts^4/8 ts^3/6; ts^4/8 ts^3/3 ts^2/2; ts^3/6 ts^2/2 ts]; % but we still use q (p185)
xest=zeros(3,300);
xest_curr=zeros(3,1);
for i=[1:1:300]
xest_pre=phi*xest_curr;
p_pre = phi*p*phi'+q;
y=sn(:,i)-h*xest_pre;
ycov=h*p_pre*h'+r;
k=p_pre*h'*inv(ycov);
xest(:,i)=xest_pre+k*y;
p=(idn-k*h)*p_pre;
xest_curr=xest(:,i);
end
sest=zeros(1,300);
sest=h*xest;
plot(sest,'r');
hold on;
plot(s,'g');
hold on;
plot(sn,'b');
hold off;
执行结果如下图:
第一张图是全貌,看不出啥;
第二张图是开始约40个点,滤波器输出慢慢靠近理想值;
第三张图是最后约50个点,滤波器输出和理想值几乎重合。
下一次,我们将看到如何利用已知的重力加速度g,以一阶多项式的卡尔曼滤波器解决该问题。
相关文章
- matlab axis画圆,使用MATLAB中axis实现图形坐标控制-Go语言中文社区
- matlab画图标签,Matlab绘图
- matlab 稀疏矩阵 乘法,Matlab 矩阵运算[通俗易懂]
- 遗传算法经典实例matlab代码_遗传算法编码方式
- matlab保存图片函数后突变分辨变化,MATLAB总结 – 图片保存「建议收藏」
- 遗传算法matlab程序简单实例_遗传算法的matlab实现
- matlab for循环语句实例_matlab如何循环
- 用matlab画三维图实例_cad绘制3d图形的教程
- matlab画三维约束图命令,Matlab画三维图的方法[通俗易懂]
- matlab 马赫带效应,matlab图像处理基础实例
- R语言爬虫教程与实例操作:如何爬取基金与Pubmed网站信息
- Kingbase国产化数据库数据迁移:oracle11g数据库转库人大金仓数据库实例演示
- SQL开发知识:SQLserver中cube:多维数据集实例详解
- Apache的Mina简单实例详解编程语言
- python正则表达式+正则大量实例详解编程语言
- MSSQL添加管理账户实例分享(mssql 添加管理账户)
- php+mysql写的简单留言本实例代码
- JS与PHP向函数传递可变参数的区别实例代码
- C#实现简单打印的实例代码
- 7款吸引人眼球的jQuery/CSS3特效实例分享
- js导入导出excel(实例代码)
- Javascript和Java获取各种form表单信息的简单实例