用matlab实现卡尔曼滤波

卡尔曼滤波是一种常用的状态估计方法，它可以用于对系统状态进行预测和更新。下面是一个简单的 MATLAB 实现：

假设我们要对一个一维系统进行状态估计，其状态方程为：

$x_k = Ax_{k-1} + w_k$

其中，$A$ 是状态转移矩阵，$w_k$ 是状态转移时的噪声。我们还有一个观测方程：

$z_k = Hx_k + v_k$

其中，$H$ 是观测矩阵，$v_k$ 是观测时的噪声。

首先，我们需要初始化一些参数：

$Q$：状态转移噪声的方差

$R$：观测噪声的方差

$P_0$：初始状态误差协方差矩阵

$x_0$：初始状态向量

$A$：状态转移矩阵

$H$：观测矩阵

然后，我们可以按照以下步骤进行卡尔曼滤波：

1. 预测状态向量和误差协方差矩阵：

$\hat{x}_{k|k-1} = A\hat{x}_{k-1|k-1}$

$P_{k|k-1} = AP_{k-1|k-1}A^T + Q$

2. 计算卡尔曼增益：

$K_k = P_{k|k-1}H^T(HP_{k|k-1}H^T + R)^{-1}$

3. 更新状态向量和误差协方差矩阵：

$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k(z_k - H\hat{x}_{k|k-1})$

$P_{k|k} = (I - K_kH)P_{k|k-1}$

最后，我们可以将以上步骤封装成一个函数，如下所示：

function [x,P] = kalman_filter(z,Q,R,A,H,x0,P0)
% z: measurement vector
% Q: process noise covariance matrix
% R: measurement noise covariance matrix
% A: state transition matrix
% H: measurement matrix
% x0: initial state vector
% P0: initial state covariance matrix
% x: updated state vector
% P: updated state covariance matrix

% initialization
x = x0;
P = P0;

for k = 1:length(z)
    % prediction
    x_pred = A*x;
    P_pred = A*P*A' + Q;
    
    % update
    K = P_pred*H'/(H*P_pred*H' + R);
    x = x_pred + K*(z(k) - H*x_pred);
    P = (eye(size(K*H)) - K*H)*P_pred;
end
end

使用该函数进行卡尔曼滤波的示例代码如下：

% generate simulated data
A = 1.2;
Q = 1;
H = 1;
R = 10;
x0 = 0;
P0 = 1;
N = 100;
w = sqrt(Q)*randn(N,1);
v = sqrt(R)*randn(N,1);
x = zeros(N,1);
z = zeros(N,1);
x(1) = x0;
z(1) = H*x(1) + v(1);
for k = 2:N
    x(k) = A*x(k-1) + w(k);
    z(k) = H*x(k) + v(k);
end

% apply Kalman filter
[x_filt,P_filt] = kalman_filter(z,Q,R,A,H,x0,P0);

% plot results
t = 1:N;
figure;
plot(t,x,t,z,t,x_filt,'--');
legend('true state','measurement','Kalman filter');