扩展卡尔曼滤波仿真实例
一、 问题描述
如图 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
最新资源