简单无迹kalman的matlab程序,卡尔曼滤波原理及应用——MATLAB仿真
时间: 2023-10-26 11:06:06 浏览: 167
卡尔曼滤波原理及应用-MATLAB仿真
5星 · 资源好评率100%
以下是一个简单的无迹卡尔曼滤波的 Matlab 程序:
```matlab
% 状态转移矩阵
F = [1 1; 0 1];
% 测量矩阵
H = [1 0];
% 过程噪声协方差矩阵
Q = [0.1 0; 0 0.1];
% 测量噪声协方差矩阵
R = 1;
% 初始状态和协方差矩阵
x_0 = [0; 0];
P_0 = [1 0; 0 1];
% 生成随机数种子
rng(0);
% 生成状态和测量噪声
v = mvnrnd([0 0], Q, 100);
w = mvnrnd(0, R, 100);
% 生成模拟数据
x = zeros(2, 100);
y = zeros(1, 100);
x(:, 1) = x_0;
y(1) = H * x_0 + w(1);
for k = 2:100
x(:, k) = F * x(:, k-1) + v(k, :)';
y(k) = H * x(:, k) + w(k);
end
% 无迹卡尔曼滤波
n = 2;
m = 1;
alpha = 1e-3;
beta = 2;
kappa = 0;
lambda = alpha^2 * (n + kappa) - n;
c = n + lambda;
W_m = [lambda/c 0.5/c*ones(1,2*n)];
W_c = W_m;
W_c(1) = W_c(1) + (1 - alpha^2 + beta);
X = zeros(n, 2*n+1);
Y = zeros(m, 2*n+1);
X(:, 1) = x_0;
P = P_0;
for k = 2:100
% 预测
X_ = F * X;
P_ = F * P * F' + Q;
% 采样
X_s = chol(P_,'lower') * [zeros(n,1) sqrt(c)*eye(n) -sqrt(c)*eye(n)];
X_s = X_s + X_*ones(1,2*n+1);
% 加权平均
X_m = X_s * W_m';
P_m = zeros(n, n);
for j = 1:2*n+1
P_m = P_m + W_c(j) * (X_s(:,j)-X_m)*(X_s(:,j)-X_m)';
end
P_m = P_m + R;
% 更新
K = P_m * H' / (H * P_m * H' + R);
X = X_m + K * (y(k) - H * X_m);
P = P_m - K * H * P_m;
end
% 画图
t = 1:100;
figure;
plot(t, x(1,:), 'b', t, X(1,:), 'r');
xlabel('时间');
ylabel('状态');
legend('真实状态', '估计状态');
```
程序中使用了一个一维的状态和一个一维的测量,状态转移矩阵 `F` 是一个 $2 \times 2$ 的矩阵,其中第一行表示状态的位置和速度,第二行表示状态的速度。测量矩阵 `H` 是一个 $1 \times 2$ 的矩阵,表示测量结果只包含状态的位置信息。
程序中使用了一个随机数种子,以便多次运行程序时生成相同的随机数序列。程序首先生成状态和测量噪声,然后生成模拟数据。接下来使用无迹卡尔曼滤波对模拟数据进行估计,并将真实状态和估计状态画在同一个图中进行比较。
关于卡尔曼滤波原理及应用,可以参考《统计信号处理》一书。
阅读全文