以下为《扩展卡尔曼滤波抛物线实例_0615》的无排版文字预览，完整格式请下载

下载前请仔细阅读文字预览以及下方图片预览。图片预览是什么样的，下载的文档就是什么样的。

扩展卡尔曼滤波仿真实例

问题描述

如图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;

y = y + vy*Ts;

vy = vy + (ky*vy^2-g+day*randn(1))*Ts;

X(k,:) = [x, vx, y, vy];

end

figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on

% figure(2), plot(X(:,2:2:4))

% 构造量测量

mrad = 0.001;

dr = 10; dafa = 10*mrad; % 量测噪声

for k=1:len

r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);

a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);

Z(k,:) = [r, a];

end

figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*')

% ekf 滤波

Qk = diag([0; dax; 0; day])^2;

Rk = diag([dr; dafa])^2;

Xk = zeros(4,1);

Pk = 100*eye(4);

X_est = X;

for k=1:len

Ft = JacobianF(X(k,:), kx, ky, g);

Hk = JacobianH(X(k,:));

fX = fff(X(k,:), kx, ky, g, Ts);

hfX = hhh(fX, Ts);

[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,: 内容过长，仅展示头部和尾部部分文字预览，全文请查看图片预览。 (x^2+y^2);

a = atan(x/y);

hfX = [r; a];

function [Xk, Pk, Kk] = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 滤波函数

Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;

Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz^-1;

Xk = fXk_1 + Kk*Zk_hfX;

Pk = Pkk_1 - Kk*Pzz*Kk';

图2 仿真结果

[文章尾部最后300字内容到此结束,中间部分内容请查看底下的图片预览]

以上为《扩展卡尔曼滤波抛物线实例_0615》的无排版文字预览，完整格式请下载

下载前请仔细阅读上面文字预览以及下方图片预览。图片预览是什么样的，下载的文档就是什么样的。