写一个matlab语言程序 EKF滤波 状态方程为非线性 测量方程为线性的代码

以下是一个基于Matlab语言的EKF滤波器程序，其中状态方程为非线性，测量方程为线性：

% EKF滤波器程序，状态方程为非线性，测量方程为线性
% 状态方程：x(k+1) = f(x(k), u(k)) + w(k)
% 测量方程：z(k) = H*x(k) + v(k)

% 初始化
x0 = [0; 0]; % 初始状态向量
P0 = diag([1, 1]); % 初始协方差矩阵
Q = eye(2); % 状态噪声协方差矩阵
R = 1; % 测量噪声协方差

% 状态转移函数
f = @(x, u) [x(1)+x(2)*u; x(2)-x(1)*u^2];

% 测量函数
H = [1, 0; 0, 1];

% 生成随机数据
N = 100; % 数据量
u = randn(N, 1); % 控制向量
w = mvnrnd([0; 0], Q, N)'; % 状态噪声
v = sqrt(R)*randn(N, 1); % 测量噪声
x = zeros(2, N); % 状态向量
z = zeros(1, N); % 测量向量
x(:, 1) = x0; % 赋初值
z(1) = H*x(:, 1) + v(1); % 赋初值
for k = 2:N
    x(:, k) = f(x(:, k-1), u(k-1)) + w(:, k); % 状态转移
    z(k) = H*x(:, k) + v(k); % 测量
end

% EKF滤波器
x_hat = zeros(2, N); % 估计状态向量
P = zeros(2, 2, N); % 估计协方差矩阵
x_hat(:, 1) = x0; % 初始估计
P(:, :, 1) = P0; % 初始估计协方差矩阵
for k = 2:N
    % 预测步骤
    x_pred = f(x_hat(:, k-1), u(k-1)); % 状态预测
    F = [1+u(k-1)^2, u(k-1); -2*u(k-1), 1]; % 状态转移矩阵
    P_pred = F*P(:, :, k-1)*F' + Q; % 协方差矩阵预测
    
    % 更新步骤
    K = P_pred*H'/(H*P_pred*H' + R); % 卡尔曼增益
    x_hat(:, k) = x_pred + K*(z(k) - H*x_pred); % 状态估计
    P(:, :, k) = (eye(2) - K*H)*P_pred; % 协方差矩阵估计
end

% 绘图
figure
subplot(2, 1, 1)
plot(1:N, x(1, :), 'b', 1:N, x_hat(1, :), 'r')
xlabel('时间')
ylabel('状态量1')
legend('真实值', '估计值')
subplot(2, 1, 2)
plot(1:N, x(2, :), 'b', 1:N, x_hat(2, :), 'r')
xlabel('时间')
ylabel('状态量2')
legend('真实值', '估计值')

注：以上程序仅供参考，具体实现可能需要根据具体情况进行调整，特别是状态转移函数和测量函数的定义需要根据具体问题进行修改。