6维ukf ekf 比较展示matlab代码
时间: 2023-06-26 21:08:34 浏览: 43
以下是一个简单的例子,比较了6维EKF和UKF的性能。假设我们有一个运动方程为:
x_dot = A * x + B * u + w
其中,x 是状态向量,u 是输入向量,w 是过程噪声。同时,我们有一个观测方程为:
y = C * x + v
其中,y 是观测向量,v 是观测噪声。我们的目标是估计状态向量 x,同时考虑过程噪声和观测噪声的影响。
首先是6维EKF的实现:
```matlab
% 系统参数
A = [1 1 0 0 0 0; 0 1 0 0 0 0; 0 0 1 1 0 0; 0 0 0 1 0 0; 0 0 0 0 1 1; 0 0 0 0 0 1];
B = [1 0; 1 0; 0 1; 0 1; 0 0; 0 0];
C = [1 0 0 0 0 0; 0 0 1 0 0 0];
Q = diag([0.1^2, 0.1^2, 0.01^2, 0.01^2, 0.001^2, 0.001^2]);
R = diag([0.1^2, 0.1^2]);
% 初始化
x_hat = [0; 0; 0; 0; 0; 0];
P = diag([1, 1, 1, 1, 1, 1]);
% 模拟数据
T = 100;
u = randn(2, T);
w = mvnrnd(zeros(6,1), Q, T)';
v = mvnrnd(zeros(2,1), R, T)';
x = zeros(6, T);
y = zeros(2, T);
for t = 1:T
x(:,t) = A * x_hat + B * u(:,t) + w(:,t);
y(:,t) = C * x(:,t) + v(:,t);
x_hat = A * x_hat + B * u(:,t);
% EKF
F = eye(6) + A * delta_t;
H = C;
x_pred = A * x_hat + B * u(:,t);
P_pred = F * P * F' + Q;
K = P_pred * H' * inv(H * P_pred * H' + R);
x_hat = x_pred + K * (y(:,t) - H * x_pred);
P = (eye(6) - K * H) * P_pred;
end
% 绘图
figure;
subplot(2,1,1);
plot(x(1,:), 'b');
hold on;
plot(x_hat(1,:), 'r');
ylabel('x_1');
legend('真实值', '估计值');
subplot(2,1,2);
plot(x(3,:), 'b');
hold on;
plot(x_hat(3,:), 'r');
ylabel('x_3');
legend('真实值', '估计值');
```
接下来是6维UKF的实现:
```matlab
% 系统参数
A = [1 1 0 0 0 0; 0 1 0 0 0 0; 0 0 1 1 0 0; 0 0 0 1 0 0; 0 0 0 0 1 1; 0 0 0 0 0 1];
B = [1 0; 1 0; 0 1; 0 1; 0 0; 0 0];
C = [1 0 0 0 0 0; 0 0 1 0 0 0];
Q = diag([0.1^2, 0.1^2, 0.01^2, 0.01^2, 0.001^2, 0.001^2]);
R = diag([0.1^2, 0.1^2]);
% 初始化
x_hat = [0; 0; 0; 0; 0; 0];
P = diag([1, 1, 1, 1, 1, 1]);
% UKF 参数
alpha = 1e-3;
beta = 2;
kappa = 0;
lambda = alpha^2 * (6 + kappa) - 6;
n = 6;
m = 2;
gamma = sqrt(n + lambda);
Wm = [lambda/(n+lambda) 0.5/(n+lambda)*ones(1,2*n) ];
Wc = Wm;
Wc(1) = Wc(1) + (1-alpha^2+beta);
U = chol(n+lambda)*sqrt(Wm);
% 模拟数据
T = 100;
u = randn(2, T);
w = mvnrnd(zeros(6,1), Q, T)';
v = mvnrnd(zeros(2,1), R, T)';
x = zeros(6, T);
y = zeros(2, T);
for t = 1:T
x(:,t) = A * x_hat + B * u(:,t) + w(:,t);
y(:,t) = C * x(:,t) + v(:,t);
x_hat = A * x_hat + B * u(:,t);
% UKF
[X, Wm, Wc] = ukf(x_hat, P, gamma);
X_pred = A * X + B * u(:,t);
Y_pred = C * X_pred;
P_pred = Wc(1) * (X_pred - x_hat) * (X_pred - x_hat)' + Q;
Pxy = zeros(n,m);
for i = 1:2*n+1
Pxy = Pxy + Wc(i) * (X(:,i) - x_hat) * (Y_pred(:,i) - C * X_pred)';
end
K = Pxy * inv(P_pred + R);
x_hat = X_pred + K * (y(:,t) - Y_pred);
P = P_pred - K * Pxy';
end
% 绘图
figure;
subplot(2,1,1);
plot(x(1,:), 'b');
hold on;
plot(x_hat(1,:), 'r');
ylabel('x_1');
legend('真实值', '估计值');
subplot(2,1,2);
plot(x(3,:), 'b');
hold on;
plot(x_hat(3,:), 'r');
ylabel('x_3');
legend('真实值', '估计值');
```
在这个例子中,我们可以看到,UKF 的表现比 EKF 更加准确和稳定。
相关推荐
![](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)