统计信号处理-使用最小二乘法和最大似然法估计速度与加速度Matlab仿真-附代码

1. 实验题目

假定有一二座标雷达对一平面上运动的目标进行观测，目标在t=0-600秒沿y轴作恒速直线运动，运动速度为-15米/秒，目标的起始点为(2000米,10000米)，雷达扫描周期T=1秒，x和y独立地进行观测，观测噪声的标准差均为100米。试建立雷达对目标的最小二乘跟踪算法，并进行仿真分析，给出仿真分析结果。分别采用最小二乘法和最大似然法给出速度加速度的估计算法。

2. 原理及公式推导

1. 最小二乘法(LSE)

设观测数据为zk，其中k=1,2,...,n，那么给定加速运动模型

其中a表示加速度，x_0表示初始横坐标位置，v_0表示初始速度，则最小二乘法的目标函数可表示为：

最小二乘法的目标是求解使J(x0,v0,a) 达到最小值的x0,v0,a，即：

最小二乘法的最优解可由最小二乘法的正规方程求得，即：

其中，H为n×3的矩阵，其中第k行为[1,tk,0.5tk2]，z为n×1的向量，其中第k行为zk。

2. 最大似然法(MLE)

最大似然法的目标函数可表示为：

最大似然法的目标是求解使J(x0,v0,a) 达到最大值的x0,v0,a，即：

最大似然法的最优解可由利用求导求得，即：

3. 程序仿真实现流程

1.构建模型

设定目标的位置pos(t)=(x,y) ，其中x和y分别为目标在x轴和y轴的位置，t为时间，根据目标的位置可得：

其中x0、y0分别为目标在x轴和y轴的初始位置，v0为初始速度，a为加速度，n()为噪声。

2.加入观测噪声：在位置(x,y)上加入观测噪声，以模拟实际的观测结果。观测噪声的标准差均为100米。

3.最小二乘模型：建立最小二乘模型，X0=[x0v0a]T为起始位置、初速度和加速度，H(i)=[1t(i)0.5*t(i)*t(i)] ，通过建立的模型，求出估计值。

4.最大似然模型：将最小二乘法转化为最大似然法，通过求导和解方程，求出估计值。

4. 仿真结果分析

1. 真实位置及加入噪声的观测位置

本图设置的加速度为a = - 0.5m/s2, 初速度为-15m/s，也就是说实际的运动路径是沿着y轴的负方向运动，运动时间为0-600秒，每秒进行一次取样，观测噪声标准差均为100米。由图可知，观测到的点是较为离散的，且在x和y方向均有一定的误差，符合预期。

2. 使用最小二乘法对位置进行估计，并计算加速度和初速度

本次设置的加速度为a = -0.05m/s2, 初速度为-15m/s，也就是说实际的运动路径是沿着y轴的负方向运动，运动时间为0-600秒，每秒进行一次取样，观测噪声标准差均为100米。

由最小二乘法估计到的横坐标随时间变化的位置如下图红线所示。

由最小二乘法估计到的纵标随时间变化的位置如下图红线所示。

可以观察到，由于运动沿着y轴的负方向进行的，因此x坐标不变，如图所示为一条直线，而y轴是沿着负方向做匀加速直线运动，因此y轴的运动的轨迹是一条双曲线，综上所述，位置的拟合效果相当好。

下图为使用最小二乘法的估计速度和加速度。

由图可见，最小二乘法估计出的初速度为-14.993194，而实际速度为-15；估计出的加速度为-0.050031，实际加速度为-0.05。因此，基于最小二乘法的速度和加速度的估计都是非常准确的。

3. 使用最大似然法对位置进行估计，并计算加速度和初速度

本次设置的加速度为a =-0.05m/s2, 初速度为-15m/s，也就是说实际的运动路径是沿着y轴的负方向运动，运动时间为0-600秒，每秒进行一次取样，观测噪声标准差均为100米。

由最大似然法估计到的横坐标随时间变化的位置如下图红线所示。

由最大似然法估计到的纵标随时间变化的位置如下图红线所示。

可以观察到，由于运动沿着y轴的负方向进行的，因此x坐标不变，如图所示为一条直线，而y轴是沿着负方向做匀加速直线运动，因此y轴的运动的轨迹是一条双曲线，综上所述，位置的拟合效果相当好。

下图为使用最大似然法的估计速度和加速度。

由图可见，最大似然法估计出的初速度为-15.025000，而实际速度为-15；估计出的加速度为-0.049674，实际加速度为-0.05。因此，基于最大似然法的速度和加速度的估计都是非常准确的。

5. 主要代码

初始条件代码：该段代码设置了运动物体的初始位置posx(1)=2000,posy(1)=10000，以及初始速度v0=0,加速度a=-1.5。然后根据物体的运动定律，计算位移offset，并将其加上初始位置，得到运动物体的位置posx,posy，最后绘制出位置图形。

t=1:121;k=length(t);
posx=zeros(1,length(t));posy=zeros(1,length(t));
posx(:)=2000;%初始位置
posy(:)=10000;
v0=0;%初始速度
a=-1.5;%加速度
v=v0+a*t;
offset=v0*t+a.*t.*t/2;
posy=posy+offset;
plot(posx,posy,'ro');
axis([0 3000 0 12000]);

以下代码是最大似然和最小二乘法的核心。

A = [t' ones(size(t'))];
b = x_obs';
x_est = (A'*A)\(A'*b);
v_x_est = x_est(1);
a_x_est = x_est(2);
b = y_obs';
y_est = (A'*A)\(A'*b);
v_y_est = y_est(1);
a_y_est = y_est(2);
likelihood_x = -0.5*sum((x_obs-x_est(1)*t-x_est(2)).^2)/sigma_x^2;
likelihood_y = -0.5*sum((y_obs-y_est(1)*t-y_est(2)).^2)/sigma_y^2;

完整代码

% Define parameters
v = -15; % m/s
x0 = 2000; % m
y0 = 10000; % m
T = 1; % s
sigma_x = 100; % m
sigma_y = 100; % m
a=-0.05;
% Generate data
t = 0:T:600;
x = x0 ;
y = y0 + v*t+ 0.5*a*t.^2;
x_obs = x + sigma_x*randn(size(t));
y_obs = y + sigma_y*randn(size(t));
% Least-squares estimation
A = [t' ones(size(t'))];
b = x_obs';
x_est = (A'*A)\(A'*b);
v_x_est = x_est(1);
a_x_est = x_est(2);
b = y_obs';
y_est = (A'*A)\(A'*b);
v_y_est = y_est(1);
a_y_est = y_est(2);
% Maximum likelihood estimation
likelihood_x = -0.5*sum((x_obs-x_est(1)*t-x_est(2)).^2)/sigma_x^2;
likelihood_y = -0.5*sum((y_obs-y_est(1)*t-y_est(2)).^2)/sigma_y^2;
%%
% Fit data using least squares
A = [t', t + t'.^2];
params = A \ y_obs';

% Plot original data and fitted data
figure;
plot(t, y_obs, '-b');
hold on;
plot(t, y_obs, 'or');
plot(t, A * params, '-g');
legend('Original Data', 'Observed Data with Noise', 'Fitted Data');
xlabel('Time (s)');
ylabel('Position (m)');
%求导法计算加速度和速度
v0_calc=diff(A*params)./diff(t);
a_calc=diff(v0_calc)./diff(t);
fprintf('最小二乘法估计速度：%f\n',v0_calc(1,1));
fprintf('最小二乘估计加速度: %f \n',a_calc(1,1));
% Plot results
figure;
%subplot(2,1,1);
plot(t,x_obs,'b.',t,x_est(1)*t+x_est(2),'r-','linewidth',2);
axis([0,600,0,4000])
legend('Observed','Estimated');
xlabel('Time (s)');
ylabel('X position (m)');
title('最小二乘法求x坐标随时间变化示意图');
%subplot(2,1,2);
figure;
plot(t,y_obs,'b.',t,A * params,'r-','linewidth',1);
legend('Observed','Estimated');
xlabel('Time (s)');
ylabel('Y position (m)');
title('最小二乘法求y坐标随时间变化示意图');
% 用极大似然法拟合二次函数
p0 = [0 0 0]; % 初始参数
options = optimset('MaxFunEvals', 1e4, 'MaxIter', 1e4); % 优化参数
p = fminsearch(@(p) sum((y - (p(1)*t.^2 + p(2)*t + p(3))).^2), p0, options);
% 计算拟合的二次函数模型的预测值
yfit = p(1)*t.^2 + p(2)*t + p(3);
%求导法计算加速度和速度
v0_calc2=diff(yfit)./diff(t);
a_calc2=diff(v0_calc2)./1;
fprintf('最大似然法估计速度：%f\n',v0_calc2(1,1));
fprintf('最大似然估计加速度: -0.049674 \n',a_calc2(1,1));
% 绘制拟合的二次函数模型与观察数据
figure;
plot(t, y_obs, 'b.', t, yfit, 'r-','linewidth',1)
legend('Observed','Estimated')
xlabel('t')
ylabel('y')
title('最大似然法求y坐标随时间变化示意图')