无迹卡尔曼滤波matlab程序
时间: 2023-05-25 21:05:13 浏览: 51
function [x_pre, P_pre, x_post, P_post] = ukf(f, x_pre, P_pre, h, z, Q, R, alpha, beta, kappa)
%输入:
%f:状态方程,形如 [x_pre] = f(x_pre),其中 x_pre 是 t-1 时刻的状态
%x_pre:t-1 时刻的状态
%P_pre:t-1 时刻的状态协方差矩阵
%h:测量方程,形如 [z] = h(x_post),其中 x_post 是 t 时刻的状态
%z:t 时刻的观测值
%Q:过程噪声协方差矩阵
%R:观测噪声协方差矩阵
%alpha, beta, kappa:UKF 参数
%输出:
%x_pre:t-1 时刻的状态
%P_pre:t-1 时刻的状态协方差矩阵
%x_post:t 时刻的状态
%P_post:t 时刻的状态协方差矩阵
n = length(x_pre); %状态量的维度
m = length(z); %观测量的维度
%计算 sigma 点
S = chol(P_pre, 'lower');
W(:, 1) = x_pre;
for i = 2:(2 * n + 1)
if i <= n+1
W(:, i) = x_pre + sqrt(n + kappa) * S(:, i-1);
else
W(:, i) = x_pre - sqrt(n + kappa) * S(:, i-n-1);
end
end
%计算权重系数
c = alpha^2 * (n + kappa) - n;
W(1:end, 1) = W(1:end, 1) * c / (n + c);
W(1:end, 2:end) = W(1:end, 2:end) * (1 / (2 * (n + c)));
%预测状态和协方差矩阵
x_pre = sum(W(1:end, :), 2);
W1 = W(1:end, 2:end) - repmat(W(1:end, 1), 1, 2*n);
P_pre = W1 * W1.' + Q;
%计算 sigma 点的观测值
for i = 1:(2*n+1)
Z_pre(:, i) = h(W(:, i));
end
%计算观测值均值和协方差矩阵
z_pre = sum(Z_pre(1:end, :), 2);
Z1 = Z_pre(1:end, 2:end) - repmat(Z_pre(1:end, 1), 1, 2*n);
Pzz = Z1 * Z1.' + R;
Pzx = W1 * Z1.';
%计算 Kalman 增益和状态和协方差矩阵更新值
K = Pzx * inv(Pzz);
x_post = x_pre + K * (z - z_pre);
P_post = P_pre - K * Pzz * K.';