用matlab写一个卡尔曼滤波函数

以下是一个简单的基于Matlab的卡尔曼滤波器函数：

function [xhat, Phat] = kalman_filter(y, A, B, C, Q, R, x0, P0)
% y: measurement vector
% A, B, C: state space matrices
% Q, R: covariance matrices
% x0, P0: initial state and covariance

% Initialization
n = length(x0);
xhat = zeros(n, length(y));
Phat = zeros(n, n, length(y));
xhat(:,1) = x0;
Phat(:,:,1) = P0;

% Kalman filter
for k = 2:length(y)
    % Time update (predict)
    xhat(:,k) = A*xhat(:,k-1) + B*u(k-1);
    Phat(:,:,k) = A*Phat(:,:,k-1)*A' + Q;

    % Measurement update (correct)
    K = Phat(:,:,k)*C'/(C*Phat(:,:,k)*C' + R);
    xhat(:,k) = xhat(:,k) + K*(y(k) - C*xhat(:,k));
    Phat(:,:,k) = (eye(n) - K*C)*Phat(:,:,k);
end

end

其中，输入参数为：

- `y`：测量向量
- `A`：状态转移矩阵
- `B`：输入矩阵
- `C`：观测矩阵
- `Q`：过程噪声协方差矩阵
- `R`：测量噪声协方差矩阵
- `x0`：初始状态向量
- `P0`：初始状态协方差矩阵

输出参数为：

- `xhat`：估计的状态向量序列
- `Phat`：估计的状态协方差矩阵序列

在实际应用中，需要根据具体问题调整卡尔曼滤波器的参数，如状态转移矩阵、观测矩阵、过程噪声协方差矩阵、测量噪声协方差矩阵等。