计算机仿真实验1 最小二乘估计器仿真实验

学习并熟悉最小二乘估计器的设计原理和方法。
- 构建最小均方误差（MMSE）估计
  $J(k) = \sum\limits_{i=1}^k \left[z(i) - H(i)\theta\right]^T\left[z(i) - H(i)\theta\right] \\ = \sum\limits_{i=1}^k \left[z(i)^Tz(i) - \theta^TH(i)^Tz(i) - z(i)^TH(i)\theta +\theta^TH(i)^TH(i)\theta \right]$
- 对 $\theta$ 的梯度为 $0$
  $\dfrac{\partial J(k)}{\partial \theta} = - H(i)^Tz(i) - H(i)^Tz(i) + 2H(i)^TH(i)\theta = 0 \\ \hat{\theta_k} = \left[ (H^k)^TH^k\right]^{-1} (H^k)^Tz^k$
- 均方估计误差
  $\hat{\theta_k}-\theta = \left[ (H^k)^TH^k\right]^{-1} (H^k)^T(H^k\theta+w)-\theta \\ = \left[ (H^k)^TH^k\right]^{-1} (H^k)^Tw$
考虑一个线性回归随机系统： $y_i = \varphi_i^T\theta + w_i, i = 1,2,\cdots,k$ 其中： $\{y_i \in \mathbb{R}, i = 1,2,\cdots,k\}$ 是一维输出观测序列 $\theta \in \mathbb{R}^n$ 是 $n$ 维待估计的参数 $\{\varphi_i \in \mathbb{R}^n, i = 1,2,\cdots,k\}$ 是 $n$ 维的输入向量序列 $\{w_i \in \mathbb{R}, i = 1,2,\cdots,k\}$ 是独立同分布的一维高斯噪声，均值为 $0$ ，方差为 $\sigma^2$ 针对该系统，设计出参数估计的递推最小二乘估计器。
- 递推最小二乘估计器
  $\hat{\theta}(k+1) = \hat{\theta}(k) + W(k+1)\left[ Z(k+1) - H(k+1)\hat{\theta}(k)\right]$
- 均分误差估计
  $P(k) = \left[ (H^k)^TH^k\right]^{-1}$
  $W(k+1) = P(k+1)H^T(k+1)$
  $P(k+1) = \left[P^{-1}(k) + H^T(k+1)H(k+1)\right]^{-1}$
对 $n = 2$ （即参数 $\theta$ 是一个二维向量）的情况，利用 MATLAB 软件进行仿真实验：
在给定输入输出数据的情况下，用所设计的最小二乘估计器给出不同时刻参数估计和估计误差的结果，并分析输入向量和噪声方差对估计结果的影响。

% 参数设置
N = 300; % 数据长度
theta = [2; 3]; % 真实状态
sigma_w = 1; % 噪声方差
delta = 1e6; 

% 初始化
P = delta * eye(2);
theta_hat = zeros(2, 1);
theta_hat_history = zeros(2, N);
error_history = zeros(2, N);

phi = zeros(2, N);
y = zeros(N, 1);

% 生成输入输出数据
for k = 1:N
    phi(:, k) = randn(2, 1); % 输入向量
    w = sigma_w * randn; % 噪声
    y(k) = phi(:, k)' * theta + w; % 输出观测
end

theta_hat = zeros(2, N);

%递推最小二乘估计器
for k = 2:N
    % 计算协方差矩阵
    P = eye(2) / (eye(2) / P + phi(:, k) * phi(:, k)');
    % 计算增益
    W = P * phi(:, k);
    % 预拟合
    theta_hat(:, k) = theta_hat(:, k-1) + W*( y(k) - phi(:, k)' * theta_hat(:, k-1));
    % 残差
    error_history(:, k) = theta_hat(:, k) - theta;
end

% 绘制参数估计结果
figure;
subplot(2,1,1);
plot(1:N, theta_hat(1, :), 'b', 1:N, theta(1) * ones(1, N), 'r--');
xlim([1, N]);  % 设置X轴范围为1到N
ylim([1, 3]);  % 设置Y轴范围
xlabel('时间步');
ylabel('参数 \theta_1 估计值');
legend('估计值', '真实值');

subplot(2,1,2);
plot(1:N, theta_hat(2, :), 'b', 1:N, theta(2) * ones(1, N), 'r--');
xlim([1, N]);  % 设置X轴范围为1到N
ylim([2, 4]);  % 设置Y轴范围
xlabel('时间步');
ylabel('参数 \theta_2 估计值');
legend('估计值', '真实值');

% 绘制估计误差
figure;
subplot(2,1,1);
plot(1:N, error_history(2, :), 'b', 1:N, 0 * ones(1, N), 'r--');
xlim([1, N]);  % 设置X轴范围为1到N
ylim([-2, 2]);  % 设置Y轴范围
xlabel('时间步');
ylabel('参数 \theta_2 估计误差');

subplot(2,1,2);
plot(1:N, error_history(2, :), 'b', 1:N, 0 * ones(1, N), 'r--');
xlim([1, N]);  % 设置X轴范围为1到N
ylim([-2, 2]);  % 设置Y轴范围
xlabel('时间步');
ylabel('参数 \theta_2 估计误差');
figure;

计算机仿真实验2 线性卡尔曼滤波器仿真实验

实验内容

学习并熟悉线性高斯卡尔曼滤波器的设计原理和方法。
考虑车辆沿着道路常速直线运动，自制车辆的位置和速度传感器测量输出数据。
车辆的常速直线运动的状态空间模型 $x_{k + 1} = Ax_{k} + Gw_{k}, y_{k} = Hx_{k} + v_{k}$ ，其中：

$x$ 是包含位置和速度状态向量， $y$ 是系统的输出向量；
$A = [1, T_s; 0, 1]$ 是系统矩阵， $T_s$ 是采样周期；
$G = [T_s^2/2; T_s]$ 是输入矩阵；
$H = [1, 0]$ 是输出矩阵；
输入的高斯噪声 $v_{k}$ 满足 $E[v_{k}^2] = q$ ， $q$ 为常值；
测量的高斯噪声 $v_{k}$ 满足 $E[v_{k}^2] = r$ ， $r$ 为常值；
系统状态初始值为 $x_0 = [0; 10]$ 。

针对该系统，设计出状态估计的递推卡尔曼滤波器。

预测
- 预测的先验状态估计 $\hat{\mathbf{x}}_{k|k-1} = A_k\hat{\mathbf{x}}_{k-1|k-1}$
- 预测的先验误差协方差估计 $P_{k|k-1} = A_kP_{k-1|k-1}A_k^T + GG^Tq$
校正
- 测量预拟合残差 $e_k=y_k - H_k\hat{\mathbf{x}}_{k|k-1}$
- 预拟合残差的协方差 $S_k =r+H_kP_{k|k-1}H_k^T$
- 最优卡尔曼增益 $K_k = P_{k|k-1}H_k^TS_k^{-1}$
- 更新的后验状态估计 $\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_ke_k$
- 更新的后验估计协方差 $P_{k|k} = (I - K_kH_k)P_{k|k-1}(I-K_kH_k)^T + K_kK_k^Tr$
- 测量后拟合残差 $e_{k|k} = \mathbf{y}_k-H_k\hat{\mathbf{x}}_{k|k}$

利用 MATLAB 软件进行仿真实验：在给定输入和测量数据的情况下，用所设计的卡尔曼滤波器给出不同时刻状态估计和估计误差的结果，并分析输入向量和噪声方差对估计结果的影响。

%% 1. 参数设置
Ts = 0.1;       % 采样时间
T_total = 10;   % 总时间
N = T_total / Ts; % 时间步数

% 系统矩阵
A = [1, Ts; 0, 1];  
G = [Ts^2/2; Ts];  
H = [1, 0];  

% 初始状态
x0 = [0; 20];   % [位置; 速度]
P0 = eye(2);    % 初始协方差

% 噪声方差
q = 1;        % 过程噪声方差
r = 1;        % 测量噪声方差

%% 2. 生成真实状态和含噪声观测
x_true = zeros(2, N);  % 真实状态
y_true = zeros(1, N);  % 观测值

x_true(:, 1) = x0;
for k = 1:N
    if k < N
        % 真实状态
        w = sqrt(q) * randn;
        x_true(:, k+1) = A * x_true(:, k) + G * w;
    end 
    % 系统输出
    v = sqrt(r) * randn;
    y_true(k) = H * x_true(:, k) + v;
end

%% 3. 卡尔曼滤波
x_pre = zeros(2, N);  % 估计状态
P_pre = zeros(2, 2, N); % 协方差矩阵

x_pre(:, 1) = x0;
P_pre(:, :, 1) = P0;

for k = 2:N
    % --- 预测步骤 ---
    x_before = A * x_pre(:, k-1); % 预测的先验状态估计
    P_before = A * P_pre(:, :, k-1) * A' + G * G' * q; %预测的先验误差协方差估计
    
    % --- 校正步骤 ---
    e = y_true(k) - H * x_before;  % 测量预拟合残差
    S = H * P_before * H' + r;     % 预拟合残差的协方差
    K = P_before * H' / S;         % 最优卡尔曼增益
    x_pre(:, k) = x_before + K * e;  % 更新的后验状态估计
    P_pre(:, :, k) = (eye(2) - K * H) * P_before * (eye(2) - K * H)' + K*K'*r;  % 更新的后验估计协方差
end

%% 4. 计算估计误差
error_pos = x_true(1, :) - x_pre(1, :);  % 位置误差
error_vel = x_true(2, :) - x_pre(2, :);  % 速度误差

%% 5. 可视化结果
t = (0:N-1) * Ts;

figure;
subplot(2,1,1);
plot(t, x_true(1,:), 'b', t, x_pre(1,:), 'r--', t, y_true, 'g.');
legend('真实位置', '估计位置', '观测值');
xlabel('时间 (s)'); ylabel('位置 (m)');
title('位置估计');

subplot(2,1,2);
plot(t, x_true(2,:), 'b', t, x_pre(2,:), 'r--');
legend('真实速度', '估计速度');
xlabel('时间 (s)'); ylabel('速度 (m/s)');
ylim([18, 22]);
title('速度估计');

% 绘制估计误差
figure;
subplot(2,1,1);
plot(1:N, error_pos, 'b', 1:N, 0 * ones(1, N), 'r--');
xlim([1, N]);  % 设置X轴范围为1到N
ylim([-2, 2]);  % 设置Y轴范围
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('位置估计误差');

subplot(2,1,2);
plot(1:N, error_vel, 'b', 1:N, 0 * ones(1, N), 'r--');
xlim([1, N]);  % 设置X轴范围为1到N
ylim([-2, 2]);  % 设置Y轴范围
xlabel('时间 (s)'); ylabel('速度误差 (m/s)');
title('速度估计误差');
figure;