扩展卡尔曼滤波抛物线实例.doc
需积分: 46 19 浏览量
2020-05-24
17:34:56
上传
评论 3
收藏 172KB DOC 举报
扩展卡尔曼滤波仿真实例
一、 问题描述
如图 1 所示，从空中水平抛射出的物体，初始水平速度 ，初始位置
坐标（ ）；受重力 和阻尼力影响，阻尼力与速度平方成正比，
水平和垂直阻尼系数分别为 ；还存在不确定的零均值白噪声干扰力
和 。在坐标原点处有一观测设备（不妨想象成雷达），可测得距离 （零
均值白噪声误差 ）、角度 （零均值白噪声误差 ）。
图 1 雷达观测示意图
二、 建模
系统方程：
量测方程：
选状态向量 ,量测向量
系统 Jacobian 矩阵
量测 Jacobian 矩阵
三、 Matlab 仿真
function test_ekf
kx = .01; ky = .05; % 阻尼系数
g = 9.8; % 重力
t = 10; % 仿真时间
Ts = 0.1; % 采样周期
len = fix(t/Ts); % 仿真步数
% 真实轨迹模拟
dax = 1.5; day = 1.5; % 系统噪声
X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
x = x + vx*Ts;
vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
评论0
最新资源