本文将https://blog.csdn.net/u012936940/article/details/77249245进行了优化讲解。
1.应用的系统模型：
2.在扩展卡尔曼滤波中，状态的预测以及观测值的预测由非线性函数计算得出，线性卡尔曼滤波中的状态转移矩阵 A 阵和观测矩阵 H 阵由 和
函数的雅克比矩阵代替，假设状态有n维，则求法如下：
3.扩展卡尔曼滤波与线性卡尔曼滤波过程相同，五个公式如下：
*为了方便表示两个雅可比公式，设出了 和
.
4.实例
（1）根据图中情景，选取横向位置、速度，纵向位置、速度为状态量，列出下面非线性状态方程及观测方程
（2）根据状态方程和观测方程，提前计算雅克比矩阵如下：
（3）Matlab实现
close all;
clear all;
%% 真实轨迹（模拟）
kx = 0.01; ky = 0.05; % 阻尼系数
g = 9.8; % 重力
t = 15; % 仿真时间
Ts = 0.1; % 采样周期
len = fix(t/Ts); % 仿真步数
dax = 0.3; day = 0.3; % 系统噪声
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
%% 构造雷达测量值
dr = 8; dafa = 0.1; % 测量噪声
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))*57.3 + dafa*randn(1,1);
Z(k,:) = [r, a];
end
%% EKF 扩展卡尔曼滤波
Qk = diag([0; dax; 0; day])^2;
Rk = diag([dr; dafa])^2; %W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的协方差(covariance)分别是Q，R（这里我们假设他们不随系统状态变化而变化）
Pk = 10*eye(4); %卡尔曼增益
Pkk_1 = 10*eye(4); %状态协方差矩阵
x_hat = [0,40,400,0]'; %最优状态过程变量
X_est = zeros(len,4); %最优的状态
x_forecast = zeros(4,1); %预测的状态矩阵
z = zeros(4,1); %观测矩阵
for k=1:len
% 1 状态预测
x1 = x_hat(1) + x_hat(2)*Ts;
vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts;
y1 = x_hat(3) + x_hat(4)*Ts;
vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts;
x_forecast = [x1; vx1; y1; vy1]; %预测值
% 2 观测预测
r = sqrt(x1*x1+y1*y1);
alpha = atan(x1/y1)*57.3; %57.3=180/pi,matlab用弧度不用角度
y_yuce = [r,alpha]';
% 状态 协方差矩阵（雅可比矩阵）
vx = x_forecast(2); vy = x_forecast(4);
F = zeros(4,4);
F(1,1) = 1; F(1,2) = Ts;
F(2,2) = 1-2*kx*vx*Ts;
F(3,3) = 1; F(3,4) = Ts;
F(4,4) = 1+2*ky*vy*Ts;
Pkk_1 = F*Pk*F'+Qk;
%观测 协方差矩阵（雅可比矩阵）
%观测方程的雅克比矩阵是观测量对状态量求导，这里观测量有距离和角度，状态有4个量，每一个观测量分别对4个状态求导，最终这里的雅克比矩阵是2x4的矩阵。
x = x_hat(1); y = x_hat(3);
H = zeros(2,4);
r = sqrt(x^2+y^2); xy2 = 1+(x/y)^2;
H(1,1) = x/r; H(1,3) = y/r;
H(2,1) = (1/y)/xy2; H(2,3) = (-x/y^2)/xy2;
%计算卡尔曼增益
Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1;
%校正
x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce);
Pk = (eye(4)-Kk*H)*Pkk_1;
X_est(k,:) = x_hat;
end
%%绘图
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-b'); %真实位置
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180)); %观测位置
plot(X_est(:,1),X_est(:,3), 'r'); %最优位置
xlabel('X');
ylabel('Y');
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);（4）Matlab仿真结果
