扩展卡尔曼滤波（EKF）仿真演示（西工大 严恭敏，2012-2-4）一、 问题描述如图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,:) =
