Automotive Vision_5 Tracking Moving Objects(追踪移动物体）I ( with Regression and Kalman Filter)

Motion estimation by regression

• 静止的点
  根据已知点，最小二乘法
  $\hat{x}=\frac{1}{n}{\sum }_{i=1}^n{x}_i,Var(x)={\sum }_{i=1}^n(x-x_i{)}^2$
• 固定速度
  $x=x_0+v\ast t(+noise)$
  search for ${\min }_{x_0,v}{\sum }_{i=1}^n(x_0+v{t}_i-x_i{)}^2$
• 加速度（类似）
• 矩阵算法
  
  简单的回归

% load('data.mat');
actual_x0 = 5;
actual_v = 3;
numT = 50;

sequence = actual_x0 + actual_v .* (1:numT);%50个点，固定速度
measurements = awgn(sequence, 1);%add white gaussian noise to signal

figure('Name', 'Regression Result'); hold on;
plot(1:numT, sequence, 'r*-');
plot(1:numT, measurements, 'ks');

% Implement regression here
A = [ numT sum(1:numT);
    sum(1:numT) sum((1:numT) .^2)];%教材上的矩阵
B = [ sum(measurements)
    sum( measurements .* (1:numT)) ];

X = linsolve(A, B); % A*X=B 求除法

% Calculate estimated poses
est_poses = zeros(1,numT); % estimated poses
est_poses(1) = measurements(1,1); % first pose must be trusted, no regression computed here

x0 = X(1);
v = X(2);
for t=1:numT
    est_poses(1,t)= x0 + t * v;
end

% Display predicted values
plot(1:numT, est_poses, 'bx', 'LineWidth', 2);
axis equal;

squared_mean_error = analyze_residual_error(est_poses, sequence)

第二个是用sliding window 计算
就是把求回归的长度控制在一定长度

load('data.mat');

window_length = 50;

figure('Name', 'Regression Result'); hold on;
plot(1:numT, true_poses(:,1), 'r*-');
plot(1:numT, measurements(:,1), 'ks');

% Variables to use
x0 = 0; % initial position
v = 0;  % velocity
est_poses = zeros(numT,1); % estimated poses
est_poses(1) = measurements(1,1); % first pose must be trusted, no regression computed here

% Implement regression here
for t=2:numT
    current_window_length = min(t, window_length);%窗口的长度不会超过50
    t_0 = t - current_window_length + 1;
    %还是用那个矩阵法计算
    A = [ current_window_length  sum(t_0:t) ;
        sum(t_0:t)  sum((t_0:t) .^2) ];
    
    timesteps = (t_0:t);
    B = [ sum(measurements(timesteps,1));
        sum( measurements(timesteps,1) .* (t_0:t)') ];
    
    X = linsolve(A, B);
    
    x0 = X(1);
    v = X(2);
    est_poses(t)= x0 + t * v;
end

% Display predicted values
plot(1:numT, est_poses, 'bx', 'LineWidth', 2);
axis equal;

squared_mean_error = analyze_residual_error(est_poses, true_poses)

Kalman Filter

kf.m

load('data.mat');

% initialization
initial_state = [0 3];
initial_state_uncertainty = 4; % initial uncertainty: 4m
initial_state_cov_matrix = diag([initial_state_uncertainty^2 initial_state_uncertainty^2]);%创建一个2*2的矩阵，这两个数在一个对角线上

state = initial_state';
unc = initial_state_cov_matrix;

% this vectors stores the list of estimated states (excluding the initial state).
state_history = zeros(numT, 2);

A = eye(2);%主对角线为1，其余为0,2*2矩阵
H = eye(2);

% plotting of true poses
figure('Name', 'Poses');
hold on;
plot(true_poses(:,1), true_poses(:,2), 'r*-');
plot(measurements(:,1), measurements(:,2), 'ks');

% main loop of kalman filter
Q = noise_system;
R = noise_meas;

for i = 1:numT
    % prediction: fill in your code
    u = delta_motion(i, :)';    
    state = A * state + u;
    unc = A * unc * A' + Q;
%     unc = system_matrix'*unc*system_matrix+noise_system;
    
    % innovation: fill in your code
    z = measurements(i, :)';
    
    k_gain = unc * H' * (H * unc * H' + R)^-1; 
    state = state + k_gain * (z - H * state);
    unc = (eye(2) - k_gain * H) * unc;
    
    % store the estimated state for error analysis in 'state_history'
    state_history(i, :) = state;

    % plot estimated state and covariance after innovation
    plot (state(1), state(2), 'bx');
    error_ellipse(unc, state);
end

axis equal;
mean_squared_error = analyze_state_error(state_history, true_poses)