用matlab function写kalmanfilter代码 其中y：测量信号 是1维的，xhat：估计状态是2维的，一个控制输入变量

以下是一个简单的Kalman Filter的Matlab Function代码，其中y是1维的测量信号，xhat是2维的状态估计，u是控制输入变量。

function [xhat_new, P_new] = KalmanFilter(y, xhat, u, P, Q, R, A, B, C)
% Kalman Filter implementation

% Prediction step
xhat_pred = A*xhat + B*u;
P_pred = A*P*A' + Q;

% Update step
K = P_pred*C'/(C*P_pred*C' + R);
xhat_new = xhat_pred + K*(y - C*xhat_pred);
P_new = (eye(size(A)) - K*C)*P_pred;
end

参数说明：

- y：测量信号，1维向量
- xhat：状态估计，2维向量
- u：控制输入，1维向量
- P：状态协方差矩阵，2x2矩阵
- Q：过程噪声协方差矩阵，2x2矩阵
- R：测量噪声协方差矩阵，1x1矩阵
- A：状态转移矩阵，2x2矩阵
- B：控制输入矩阵，2x1矩阵
- C：测量矩阵，1x2矩阵

使用方法：

在Matlab中运行该函数，输入y、xhat、u、P、Q、R、A、B、C参数，即可得到新的状态估计xhat_new和状态协方差矩阵P_new。