平方根容积卡尔曼滤波matlab代码

您好！以下是一个简单的基于卡尔曼滤波的平方根容积估计器的 MATLAB 代码示例：

% 定义系统模型
A = 1;              % 状态转移矩阵
C = 1;              % 观测矩阵
Q = 0.1;            % 系统过程噪声协方差
R = 1;              % 观测噪声协方差

% 初始化估计状态向量
x_hat = 0;          % 估计状态
P = 1;              % 初始化估计误差协方差

% 初始化输入输出向量和时间步长
T = 100;            % 时间步长
u = zeros(T, 1);    % 输入向量
y = zeros(T, 1);    % 输出向量

% 生成系统数据
for t = 1:T
    % 生成输入
    u(t) = sin(t/10);
    
    % 系统真实值
    x_true = A * x_hat + u(t);
    
    % 生成观测
    y(t) = C * x_true + sqrt(R) * randn;
    
    % 更新协方差矩阵
    P = A * P * A' + Q;
    
    % 计算卡尔曼增益
    K = P * C' / (C * P * C' + R);
    
    % 更新估计状态和协方差矩阵
    x_hat = x_hat + K * (y(t) - C * x_hat);    P = (eye(1) - K * C) * P;
end

% 绘制结果
t = 1:T;
plot(t, y, 'r', t, x_hat, 'b');
legend('观测值', '估计值');
xlabel('时间步长');
ylabel('数值');
title('平方根容积卡尔曼滤波估计结果');

这段代码实现了一个简单的平方根容积卡尔曼滤波器，用于估计由状态转移模型（A）和观测模型（C）描述的系统的状态。在代码中，我们使用正弦函数作为输入信号，并通过加入高斯噪声生成观测值。最后，绘制了观测值和估计值的对比结果。

请注意，这只是一个简单示例，实际应用中可能需要根据具体问题进行修改和扩展。希望对您有所帮助！如有任何问题，请随时提问。