卡尔曼滤波器实例:跟踪自由落体运动:设计与Matlab实现
2017-04-30 16:17
519 查看
[首发: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,以一阶多项式的卡尔曼滤波器解决该问题。
本文所用实例来自于以下书籍:
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的gui界面设计实例 (2008-10-03 15:47:30)matlab gui 界面 校园 分类:Matlab实例
- 实例:校园网双出口的设计与配置实现
- [设计思想]单实例设计模式的实现
- Matlab、ISE联合开发实例之中值滤波(二)FPGA硬件架构实现
- Matlab、ISE联合开发实例之中值滤波(一)Matlab实现
- 单实例设计模式的实现
- IIR滤波器设计(调用MATLAB IIR函数来实现)
- 单实例设计模式的实现
- 单实例设计模式的实现
- Android 蓝牙开发实例--蓝牙聊天程序的设计和实现
- 单实例设计模式的实现
- 基于Matlab的FIR滤波器设计与实现
- 多级树集合分裂(SPIHT)算法的过程详解与Matlab实现(8)实例演示
- 第2章 单层前向网络及LMS学习算法仿真实例 Matlab 实现
- 基于模型设计的FPGA开发与实现:滤波器设计与实现(四)Matlab中滤波器HDL代码生成优化
- 系统好友推荐实现之数据库设计实例
- 基于模型设计的FPGA开发与实现:滤波器设计与实现(三)Matlab中滤波器的HDL代码生成
- 基于模型设计的FPGA开发与实现:滤波器设计与实现(一)在Matlab中高效设计滤波器
- Matlab预测控制工具箱设计实例
- 基于JsessionId的会话跟踪登录设计与实现