vold-kalman滤波matlab代码

以下是一个基于MATLAB的Vold-Kalman滤波器的代码示例：

% 初始化变量和参数
n = 2;  % 状态维度
m = 1;  % 观测维度
dt = 0.1;  % 时间间隔

A = [1 dt;
     0 1];  % 系统矩阵

H = [1 0];  % 观测矩阵

Q = [1 0;
     0 1];  % 系统噪声协方差矩阵

R = 1;  % 观测噪声协方差矩阵

P = [1 0;
     0 1];  % 状态协方差矩阵

x = [0;
     0];  % 状态向量

z = 0;  % 观测向量

% 模拟数据
num_samples = 100;
true_pos = zeros(num_samples, 1);
measured_pos = zeros(num_samples, 1);
filtered_pos = zeros(num_samples, 1);

v = 1;
true_pos(1) = 0;
measured_pos(1) = true_pos(1) + sqrt(R) * randn();
filtered_pos(1) = measured_pos(1);

for i = 2:num_samples
    true_pos(i) = true_pos(i-1) + v * dt;
    measured_pos(i) = true_pos(i) + sqrt(R) * randn();

    % 预测
    x = A * x;
    P = A * P * A' + Q;

    % 更新
    z = measured_pos(i);
    y = z - H * x;
    S = H * P * H' + R;
    K = P * H' / S;
    x = x + K * y;
    P = (eye(n) - K * H) * P;

    filtered_pos(i) = x(1);
end

% 输出结果
for i = 1:num_samples
    fprintf('True position: %f, Measured position: %f, Filtered position: %f\n', true_pos(i), measured_pos(i), filtered_pos(i));
end

其中，`randn()` 是一个标准正态分布随机数生成函数，MATLAB自带。