贝叶斯滤波matlab代码
时间: 2023-07-10 22:33:34 浏览: 74
以下是一个简单的基于卡尔曼滤波器的贝叶斯滤波的Matlab示例代码:
```matlab
% 定义系统模型
A = [1 1; 0 1];
B = [0.5; 1];
C = [1 0];
D = 0;
Q = [0.01 0; 0 0.01];
R = 1;
x0 = [0; 0];
P0 = [1 0; 0 1];
% 定义卡尔曼滤波器
sys = ss(A, [B eye(2)], C, [D zeros(1, 2)], -1);
[kalman_ss, L, P] = kalman(sys, Q, R);
% 定义初始状态
x_hat = x0;
P_hat = P0;
% 模拟观测数据
N = 100;
y = zeros(N, 1);
x_true = zeros(N, 2);
w = sqrt(Q)*randn(N, 2);
v = sqrt(R)*randn(N, 1);
for k = 1:N
x_true(k,:) = A*x_hat + B*w(k,:)';
y(k) = C*x_true(k,:)' + v(k);
[x_hat, P_hat] = kalman_filter(y(k), x_hat, P_hat, A, B, C, D, Q, R);
end
% 绘制图形
t = 1:N;
figure;
subplot(2,1,1);
plot(t, x_true(:,1), 'b', t, y, 'r', t, kalman_ss.C*x_hat, 'g');
ylabel('Position');
legend('True', 'Observation', 'Estimate');
subplot(2,1,2);
plot(t, x_true(:,2), 'b', t, kalman_ss.D*y, 'g');
ylabel('Velocity');
xlabel('Time');
legend('True', 'Estimate');
```
其中,`kalman_filter`函数实现了卡尔曼滤波的递推公式,代码如下:
```matlab
function [x_hat, P_hat] = kalman_filter(y, x_hat, P_hat, A, B, C, D, Q, R)
% 预测状态
x_minus = A*x_hat;
P_minus = A*P_hat*A' + B*Q*B';
% 计算卡尔曼增益
K = P_minus*C'*inv(C*P_minus*C' + D*R*D');
% 更新状态
x_hat = x_minus + K*(y - C*x_minus - D*0);
P_hat = (eye(size(A)) - K*C)*P_minus*(eye(size(A)) - K*C)' + K*R*K';
end
```
该代码可以模拟一个一维自由落体的状态估计问题,其中系统模型由状态转移矩阵`A`、控制输入矩阵`B`、观测矩阵`C`、过程噪声协方差矩阵`Q`、观测噪声协方差矩阵`R`和初始状态`x0`、初始协方差矩阵`P0`定义。在模拟过程中,首先定义了一个卡尔曼滤波器`kalman_ss`,然后通过循环递推计算状态估计值`x_hat`和协方差矩阵估计值`P_hat`,并将其与真实状态和观测数据进行比较。