EKF例程 matlab

% 不含IMU误差方程的EKF滤波典型程序，适用于多次滤波的第二级
% author:Evand
% date: 2023-09-20
% Ver1
clear;clc;close all;
global T
%% initial
T = 0.1; %采样率
t = [T:T:100];
Q = 0.1diag([1,1,1]);w=sqrt(Q)randn(size(Q,1),length(t));
R = 1diag([1,1,1]);v=sqrt®randn(size(R,1),length(t));
P = 1eye(3);
P_num = zeros(length(t),size(P,1),size(P,2)); %存放每次迭代的P
X=zeros(3,length(t));
X_ekf=zeros(3,length(t));
X_ekf(1,1)=X(1,1);
Z=zeros(3,length(t)); %定义观测值形式
Z(:,1)=[X(1,1)^2/20;X(2,1);X(3,1)]+v(:,1); %观测量
%% model
X_=zeros(3,length(t));
X_(:,1)=X(:,1);
for i1 = 2:length(t)
X(:,i1) = [1 * X(1,i1-1) + (2.5 * X(1,i1-1) / (1 + X(1,i1-1).^2)) + 8 * cos(1.2(i1-1));
X(2,i1-1)+1;
(2X(3,i1-1)+1)^0.5]; %真实值
X_(:,i1) = [1 * X_(1,i1-1) + (2.5 * X_(1,