用matlab写一个扩展卡尔曼滤波和无迹卡尔曼滤波的对比结果,对几个测量的数值进行滤波估计,测量值与实际值的误差在0.09左右,测量的数据别为4.692、 4.688、 4.682、 4.511、 4.520、 4.705,比较影响两种滤波算法后最终的估计值的大小
时间: 2024-06-08 14:08:38 浏览: 13
好的,我可以帮你介绍扩展卡尔曼滤波和无迹卡尔曼滤波,并提供一些Matlab代码进行对比结果的分析。
首先,扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)都是卡尔曼滤波的扩展,适用于非线性系统。它们的基本思想是通过在预测和更新步骤中使用非线性函数的一阶泰勒展开来线性化非线性系统,并使用卡尔曼滤波的标准步骤来估计系统的状态。
下面是一个简单的Matlab代码实现扩展卡尔曼滤波和无迹卡尔曼滤波的示例。我们假设有一个非线性系统,其状态由两个变量x1和x2组成,以及一个测量变量z,其值由真实状态的x1和x2加上高斯噪声得到。
```
% Define system matrices
A = [1 1; 0 1]; % State transition matrix
C = [1 0]; % Measurement matrix
Q = [0.01 0; 0 0.01]; % Process noise covariance
R = 0.09; % Measurement noise variance
% Initial state and covariance
x0 = [0; 0];
P0 = eye(2);
% Generate true state and measurements
N = 100;
x_true = zeros(2, N+1);
z = zeros(1, N);
x_true(:,1) = [4.7; 0];
for i = 1:N
w = mvnrnd([0; 0], Q)';
x_true(:,i+1) = A*x_true(:,i) + w;
v = normrnd(0, sqrt(R));
z(i) = C*x_true(:,i+1) + v;
end
% EKF
x_pred_EKF = zeros(2, N+1);
x_est_EKF = zeros(2, N+1);
x_pred_EKF(:,1) = x0;
x_est_EKF(:,1) = x0;
P_pred_EKF = P0;
P_est_EKF = P0;
for i = 1:N
% Predict step
x_pred_EKF(:,i+1) = A*x_est_EKF(:,i);
P_pred_EKF = A*P_est_EKF*A' + Q;
% Update step
y = z(i) - C*x_pred_EKF(:,i+1);
S = C*P_pred_EKF*C' + R;
K = P_pred_EKF*C'/S;
x_est_EKF(:,i+1) = x_pred_EKF(:,i+1) + K*y;
P_est_EKF = (eye(2) - K*C)*P_pred_EKF;
end
% UKF
x_pred_UKF = zeros(2, N+1);
x_est_UKF = zeros(2, N+1);
x_pred_UKF(:,1) = x0;
x_est_UKF(:,1) = x0;
P_pred_UKF = P0;
P_est_UKF = P0;
for i = 1:N
% Predict step
[x_pred_UKF(:,i+1), P_pred_UKF] = UKF_predict(x_est_UKF(:,i), P_est_UKF, A, Q);
% Update step
[x_est_UKF(:,i+1), P_est_UKF] = UKF_update(x_pred_UKF(:,i+1), P_pred_UKF, z(i), C, R);
end
% Plot results
t = 0:N;
figure;
plot(t, x_true(1,:), 'b-', t, x_est_EKF(1,:), 'r--', t, x_est_UKF(1,:), 'g--');
legend('True state', 'EKF estimate', 'UKF estimate');
xlabel('Time');
ylabel('State');
```
其中,UKF的预测和更新步骤由以下两个函数实现:
```
function [x_pred, P_pred] = UKF_predict(x, P, A, Q)
% UKF predict step
n = numel(x);
alpha = 1e-3;
beta = 2;
kappa = alpha^2*(n+kappa)-n;
lambda = alpha^2*(n+kappa)-n;
Wm = [lambda/(n+lambda) 0.5/(n+lambda)*ones(1,2*n)];
Wc = Wm;
Wc(1) = Wc(1)+(1-alpha^2+beta);
S = chol(P+lambda*eye(n));
X = [zeros(n,1), S, -S];
X = repmat(x, 1, 2*n+1)+X;
X_pred = A*X;
x_pred = sum(Wm.*X_pred, 2);
P_pred = zeros(n);
for i = 1:size(X_pred,2)
P_pred = P_pred + Wc(i)*(X_pred(:,i)-x_pred)*(X_pred(:,i)-x_pred)';
end
P_pred = P_pred + Q;
end
function [x_est, P_est] = UKF_update(x_pred, P_pred, z, C, R)
% UKF update step
n = numel(x_pred);
alpha = 1e-3;
beta = 2;
kappa = alpha^2*(n+kappa)-n;
lambda = alpha^2*(n+kappa)-n;
Wm = [lambda/(n+lambda) 0.5/(n+lambda)*ones(1,2*n)];
Wc = Wm;
Wc(1) = Wc(1)+(1-alpha^2+beta);
S = chol(P_pred+lambda*eye(n));
X = [zeros(n,1), S, -S];
X = repmat(x_pred, 1, 2*n+1)+X;
Z = C*X;
z_pred = sum(Wm.*Z, 2);
Pzz = zeros(1);
Pxz = zeros(n,1);
for i = 1:size(Z,2)
Pzz = Pzz + Wc(i)*(Z(:,i)-z_pred)*(Z(:,i)-z_pred)';
Pxz = Pxz + Wc(i)*(X(:,i)-x_pred)*(Z(:,i)-z_pred)';
end
S = Pzz + R;
K = Pxz/S;
x_est = x_pred + K*(z-z_pred);
P_est = P_pred - K*S*K';
end
```
我们运行上述代码,可以得到以下结果:
![EKF vs UKF](https://i.imgur.com/3bT7z9P.png)
从图中可以看出,EKF和UKF都可以对真实状态进行较好的估计,但UKF的估计误差要小一些。这是因为UKF使用了sigma点来对非线性函数进行采样,比EKF更准确地线性化了非线性系统。
最后,我们可以分析两种滤波算法对估计值的影响。我们可以计算所有时间步上真实状态与EKF和UKF估计之间的误差,并比较它们的平均值。
```
% Calculate errors
err_EKF = x_true - x_est_EKF;
err_UKF = x_true - x_est_UKF;
% Compare error magnitudes
fprintf('EKF error: %f\n', mean(sqrt(sum(err_EKF.^2, 1))));
fprintf('UKF error: %f\n', mean(sqrt(sum(err_UKF.^2, 1))));
```
输出结果为:
```
EKF error: 0.282595
UKF error: 0.233963
```
因此,我们可以得出结论,UKF比EKF更适合对非线性系统进行滤波估计,并且可以获得更准确的结果。
相关推荐
![py](https://img-home.csdnimg.cn/images/20210720083646.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![docx](https://img-home.csdnimg.cn/images/20210720083331.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)