（十三）卡尔曼滤波初始化-CSDN博客

本文链接：https://blog.csdn.net/h320130/article/details/144286778

前言

滤波初始化会严重影响到初始时刻的跟踪精度，因此只有初始化进行好了，才可以利用卡尔曼滤波对目标进行跟踪。本文针对四维、六维及九维条件下的卡尔曼滤波进行了初始化，对应的原理以及代码如文中所示。（后续代码会在资源中上传）

一、四维状态变量滤波初始化

（一）基础知识

目标状态向量定义为 $x=\left [ x,\dot{x} ,y,\dot{y}\right ]{}'$ ，对应的量测函数如下：

$z=\left\{\begin{matrix} z_{1}=\sqrt{x^{2}+y^{2}}\\ z_{2}=atan\left ( \frac{y}{x} \right ) \end{matrix}\right.$

此时在 $k=2$ 时刻进行滤波的初始化，对应状态向量如下：

$\left\{\begin{matrix} \bar{x}(k)=z_{1}\left ( k \right )*cos\left ( z_{2}\left ( k \right ) \right )\\ \bar{y}(k)=z_{1}\left ( k \right )* sin\left ( z_{2}\left ( k \right ) \right ) \end{matrix}\right.$

$x\left ( 2 \right )=\left [ \bar{x}\left ( 2 \right ),(\bar{x}\left ( 2 \right )-\bar{x}\left ( 1 \right ))/T,\bar{y}\left ( 2 \right ),(\bar{y}\left ( 2 \right )-\bar{y}\left ( 1 \right ))/T\right ]{}'$

对应的 $k=2$ 时刻的协方差矩阵计算如下：

$C(k)=\begin{bmatrix} cos(z_{2}(k)) & -z_{1}(k)*cos(z_{2}(k))\\ sin(z_{2}(k))& z_{1}(k)*sin(z_{2}(k)) \end{bmatrix}$

$P_{initial}(k)=\begin{bmatrix} P_{11}(k) & P_{12}(k)\\ P_{12}(k) & P_{22}(k) \end{bmatrix}=C(k)*R\left ( k \right )*{C}'(k)$

$P(2)=\begin{bmatrix} P_{11}(2)& P_{11}(2)/T&P_{12}(2) & P_{12}(2)/T \\ P_{11}(2)/T& 2*P_{11}(2)/T^{2}& P_{12}(2)/T& 2*P_{12}(2)/T^{2}\\ P_{12}(2) & P_{12}(2)/T& P_{22}(2)& P_{22}(2)/T \\ P_{12}(2)/T& 2*P_{12}(2)/T^{2}& P_{22}(2)/T& 2*P_{22}(2)/T^{2}\end{bmatrix}$

其中， $R\left ( k \right )$ 为量测协方差矩阵。对应仿真代码如下：


n_sim = 50; %仿真时间
Ts = 1; %仿真时间间隔
nz = 2; %量测维数
x_start = [10 2 30,-1]'; %目标初始时刻状态
nf = 0.001;
np = 2; %每一位的状态变量数：CV为2；CA为3
nd = 2; %空间维数：二维or三维
nx = np*nd; %状态维数
RMSE = zeros(n_sim,1);
Montrkalo = 100;
RMSE_sum = 0;
for k = 1:Montrkalo
    %目标真实轨迹及其量测
    [F,Q]= state_tran(Ts,nd,nf,np);
    x_real = zeros(nx,n_sim);
    x_real(:,1) = x_start;
    for i = 2:n_sim
        x_real(:,i) = F*x_real(:,i-1) + Q*randn(nx,1);
    end

    z_real = zeros(nz,n_sim);
    R = diag([4,0.0001]);
    for i = 1:n_sim
        z_real(:,i) = h_calculate(x_real(:,i),np,nd)+R*randn(nz,1);
    end

    %滤波初始化
    x_filter = zeros(nx,n_sim);
    P = cell(n_sim,1);
    P(:) = {zeros(nx,nx)};

    n_initial = 2; %开始时刻
    rou_initial_1 = z_real(1,n_initial-1);
    theat_initial_1 = z_real(2,n_initial-1);
    x_initial_1 = rou_initial_1*cos(theat_initial_1);%量测转换得到的
    y_initial_1 = rou_initial_1*sin(theat_initial_1);

    rou_initial = z_real(1,n_initial);
    theat_initial = z_real(2,n_initial);
    x_initial = rou_initial*cos(theat_initial); %量测转换得到的
    y_initial = rou_initial*sin(theat_initial);

    x_filter(:,n_initial) = [x_initial,(x_initial-x_initial_1)/Ts,y_initial,(y_initial-y_initial_1)/Ts]';
    RMSE(n_initial) = sqrt((x_filter(1,n_initial)-x_real(1,n_initial))^2 + (x_filter(3,n_initial)-x_real(3,n_initial))^2);

    A = [cos(theat_initial),-rou_initial*sin(theat_initial);sin(theat_initial),rou_initial*cos(theat_initial)];
    R_trans_initial = A*R*A';

    P{n_initial} = [R_trans_initial(1,1), R_trans_initial(1,1)/Ts, R_trans_initial(1,2), R_trans_initial(1,2)/Ts;
        R_trans_initial(1,1)/Ts, 2*R_trans_initial(1,1)/(Ts^2), R_trans_initial(1,2)/Ts, 2*R_trans_initial(1,2)/(Ts^2);
        R_trans_initial(1,2), R_trans_initial(1,2)/Ts, R_trans_initial(2,2), R_trans_initial(2,2)/Ts;
        R_trans_initial(1,2)/Ts, 2*R_trans_initial(1,2)/(Ts^2), R_trans_initial(2,2)/Ts, 2*R_trans_initial(2,2)/(Ts^2) ];

    for i = n_initial+1:n_sim
        [x_filter(:,i),P{i}] = UKF(x_filter(:,i-1),z_real(:,i),P{i-1},R,F,Q,np,nd);
        RMSE(i) = sqrt((x_filter(1,i)-x_real(1,i))^2 + (x_filter(3,i)-x_real(3,i))^2);
    end
    RMSE_sum = RMSE_sum+RMSE;
end

RMSE_final = RMSE_sum/Montrkalo;

figure;
plot(x_filter(1,n_initial:end),x_filter(3,n_initial:end),'r',LineWidth=1);
hold on;
plot(x_real(1,:),x_real(3,:),'b--',LineWidth=1);
xlabel('x');
ylabel('y');
legend("滤波轨迹","真实轨迹")


figure;
plot(n_initial:n_sim,RMSE_final(n_initial:end),'b--',LineWidth=1);
xlabel('RMSE');
ylabel('Time');

（二）仿真结果

对应的目标运动轨迹及RMSE计算如下：

从仿真结果中可以看出，对应的滤波器能够实现较好的初始化，目标跟踪精度不断提升，说明了初始化的有效性。

二、六维状态变量滤波初始化

（一）基础知识

目标状态向量定义为 $x=\left [ x,\dot{x} ,y,\dot{y},z,\dot{z}\right ]{}'$ ，对应的量测函数如下：

$z=\left\{\begin{matrix} z_{1}=\sqrt{x^{2}+y^{2}+y^{2}} \\ z_{2}=atan\left ( \frac{y}{x} \right )\\ z_{3}=atan\left ( \frac{y}{\sqrt{x^{2}+y^{2}}} \right ) \end{matrix}\right.$

此时在 $k=2$ 时刻进行滤波的初始化，对应状态向量如下：

$\left\{\begin{matrix} \bar{x}(k)=z_{1}\left ( k \right )* cos\left ( z_{2}\left ( k \right ) \right )* cos(z_{3}(k))\\ \bar{y}(k)=z_{1}\left ( k \right )*sin\left ( z_{2}\left ( k \right ) \right )* cos(z_{3}(k)) \\ \bar{z}(k)=z_{1}\left ( k \right )* sin\left ( z_{3}\left ( k \right ) \right ) \end{matrix}\right.$

对应的 $k=2$ 时刻的协方差矩阵计算如下：

$C(k)=\begin{bmatrix} cos(z_{2}(k))*cos(z_{3}(k)) &-z_{1}(k)*sin(z_{2}(k))*cos(z_{3}(k)) & -z_{1}(k)*cos(z_{2}(k))*sin(z_{3}(k)) \\ sin(z_{2}(k))*cos(z_{3}(k)) & z_{1}(k)*cos(z_{2}(k))*cos(z_{3}(k))& -z_{1}(k)*sin(z_{2}(k))*sin(z_{3}(k))\\ sin(z_{3}(k)) & 0 & z_{1}(k)*cos(z_{3}(k))\end{bmatrix}$

$P_{initial}(k)=\begin{bmatrix} P_{11}(k) & P_{12}(k) &P_{13}(k) \\ P_{12}(k) & P_{22}(k) &P_{23}(k)\\ P_{13}(k) & P_{23}(k) &P_{33}(k)\end{bmatrix} =C(k)*R\left ( k \right )*{C}'(k)$

$P(2)=\begin{bmatrix} P_{11}(2)& P_{11}(2)/T&P_{12}(2) & P_{12}(2)/T&P_{13}(2)& P_{13}(2)/T \\ P_{11}(2)/T& 2*P_{11}(2)/T^{2}& P_{12}(2)/T& 2*P_{12}(2)/T^{2}& P_{13}(2)/T& 2*P_{13}(2)/T^{2}\\ P_{12}(2) & P_{12}(2)/T& P_{22}(2)& P_{22}(2)/T &P_{23}(2)& P_{23}(2)/T\\ P_{12}(2)/T& 2*P_{12}(2)/T^{2}& P_{22}(2)/T& 2*P_{22}(2)/T^{2}& P_{23}(2)/T& 2*P_{23}(2)/T^{2}\\ P_{13}(2)& P_{13}(2)/T&P_{23}(2) & P_{23}(2)/T&P_{33}(2)& P_{33}(2)/T \\ P_{13}(2)/T& 2*P_{13}(2)/T^{2}& P_{23}(2)/T& 2*P_{23}(2)/T^{2}& P_{33}(2)/T& 2*P_{33}(2)/T^{2} \end{bmatrix}$

其中， $R\left ( k \right )$ 为量测协方差矩阵。对应仿真代码如下：


n_sim = 50; %仿真时间
Ts = 1; %仿真时间间隔
nz = 3; %量测维数
x_start = [10, 2, 30, -1, 50, 2]'; %目标初始时刻状态
nf = 0.001;
np = 2; %每一位的状态变量数：CV为2；CA为3
nd = 3; %空间维数：二维or三维
nx = np*nd; %状态维数
RMSE = zeros(n_sim,1);
Montrkalo = 100;
RMSE_sum = 0;
for k = 1:Montrkalo
    %目标真实轨迹及其量测
    [F,Q]= state_tran(Ts,nd,nf,np);
    x_real = zeros(nx,n_sim);
    x_real(:,1) = x_start;
    for i = 2:n_sim
        x_real(:,i) = F*x_real(:,i-1) + Q*randn(nx,1);
    end

    z_real = zeros(nz,n_sim);
    R = diag([4,0.0001,0.0001]);
    for i = 1:n_sim
        z_real(:,i) = h_calculate(x_real(:,i),np,nd)+R*randn(nz,1);
    end

    %滤波初始化
    x_filter = zeros(nx,n_sim);
    P = cell(n_sim,1);
    P(:) = {zeros(nx,nx)};

    n_initial = 2; %开始时刻

    rou_initial_1 = z_real(1,n_initial-1);
    theat_initial_1 = z_real(2,n_initial-1);
    epsion_initial_1 = z_real(3,n_initial-1);
    x_initial_1 = rou_initial_1*cos(theat_initial_1)*cos(epsion_initial_1);%量测转换得到的
    y_initial_1 = rou_initial_1*sin(theat_initial_1)*cos(epsion_initial_1);
    z_initial_1 = rou_initial_1*sin(epsion_initial_1);

    rou_initial = z_real(1,n_initial);
    theat_initial = z_real(2,n_initial);
    epsion_initial = z_real(3,n_initial);
    x_initial = rou_initial*cos(theat_initial)*cos(epsion_initial); %量测转换得到的
    y_initial = rou_initial*sin(theat_initial)*cos(epsion_initial);
    z_initial = rou_initial*sin(epsion_initial);

    x_filter(:,n_initial) = [x_initial,(x_initial-x_initial_1)/Ts,y_initial,(y_initial-y_initial_1)/Ts,z_initial,(z_initial-z_initial_1)/Ts]';
    RMSE(n_initial) = sqrt((x_filter(1,n_initial)-x_real(1,n_initial))^2 + (x_filter(3,n_initial)-x_real(3,n_initial))^2+(x_filter(5,n_initial)-x_real(5,n_initial))^2);
    
    A = [cos(theat_initial)*cos(epsion_initial),-rou_initial*sin(theat_initial)*cos(epsion_initial),-rou_initial*cos(theat_initial)*sin(epsion_initial);
        sin(theat_initial)*cos(epsion_initial),rou_initial*cos(theat_initial)*cos(epsion_initial),-rou_initial*sin(theat_initial)*sin(epsion_initial);
        sin(epsion_initial), 0 , rou_initial*cos(theat_initial)];
    R_trans_initial = A*R*A';
    R11 = [R_trans_initial(1,1), R_trans_initial(1,1)/Ts;
        R_trans_initial(1,1)/Ts, 2*R_trans_initial(1,1)/(Ts^2)];
    R12 = [R_trans_initial(1,2), R_trans_initial(1,2)/Ts;
        R_trans_initial(1,2)/Ts, 2*R_trans_initial(1,2)/(Ts^2)];
    R13 = [R_trans_initial(1,3), R_trans_initial(1,3)/Ts;
        R_trans_initial(1,3)/Ts, 2*R_trans_initial(1,3)/(Ts^2)];
    R23 = [R_trans_initial(2,3), R_trans_initial(2,3)/Ts;
        R_trans_initial(2,3)/Ts, 2*R_trans_initial(2,3)/(Ts^2)];
    R22 = [R_trans_initial(2,2), R_trans_initial(2,2)/Ts;
        R_trans_initial(2,2)/Ts, 2*R_trans_initial(2,2)/(Ts^2)];
    R33 = [R_trans_initial(3,3), R_trans_initial(3,3)/Ts;
        R_trans_initial(3,3)/Ts, 2*R_trans_initial(3,3)/(Ts^2)];
    P{n_initial} = [R11, R12, R13;
        R12, R22, R23;
        R13, R23, R33];

    for i = n_initial+1:n_sim
        [x_filter(:,i),P{i}] = UKF(x_filter(:,i-1),z_real(:,i),P{i-1},R,F,Q,np,nd);
        RMSE(i) = sqrt((x_filter(1,i)-x_real(1,i))^2 + (x_filter(3,i)-x_real(3,i))^2 + (x_filter(5,i)-x_real(5,i))^2);
    end
    RMSE_sum = RMSE_sum+RMSE;
end

RMSE_final = RMSE_sum/Montrkalo;
figure;
plot3(x_filter(1,n_initial:end),x_filter(3,n_initial:end),x_filter(5,n_initial:end),'r',LineWidth=1);
hold on;
plot3(x_real(1,:),x_real(3,:),x_real(5,:),'b--',LineWidth=1);
xlabel('x');
ylabel('y');
zlabel('z');
legend("滤波轨迹","真实轨迹")

figure;
plot(n_initial:n_sim,RMSE_final(n_initial:end),'b--',LineWidth=1);
xlabel('RMSE');
ylabel('Time');

（二）仿真结果

对应的目标运动轨迹及RMSE计算如下：

从仿真结果中可以看出，对应的滤波器能够实现较好的初始化，目标跟踪精度不断提升，说明了初始化的有效性。

三、九维状态变量滤波初始化

（一）基础知识

目标状态向量定义为 $x=\left [ x,\dot{x} ,\ddot{x},y,\dot{y},\ddot{y},z,\dot{z},\ddot{z}\right ]{}'$ ，对应的量测函数如下：

$z=\left\{\begin{matrix} z_{1}=\sqrt{x^{2}+y^{2}+y^{2}} \\ z_{2}=atan\left ( \frac{y}{x} \right )\\ z_{3}=atan\left ( \frac{y}{\sqrt{x^{2}+y^{2}}} \right ) \end{matrix}\right.$

此时在 $k=3$ 时刻进行滤波的初始化，对应状态向量如下：

$x(2)=\begin{bmatrix} \bar{x}\left ( 3 \right )\\ (\bar{x}\left ( 3 \right )-\bar{x}\left ( 2 \right ))/T\\ ((\bar{x}\left ( 3 \right )-\bar{x}\left ( 2 \right ))/T-(\bar{x}\left ( 2 \right )-\bar{x}\left ( 1 \right ))/T)/T\\ \bar{y}\left ( 3 \right )\\ (\bar{y}\left ( 3 \right )-\bar{y}\left ( 2 \right ))/T\\ ((\bar{y}\left ( 3 \right )-\bar{y}\left ( 2 \right ))/T-(\bar{y}\left ( 2 \right )-\bar{y}\left ( 1 \right ))/T)/T\\ \bar{z}\left ( 3 \right )\\ (\bar{z}\left ( 3 \right )-\bar{z}\left ( 2 \right ))/T\\ ((\bar{z}\left ( 3 \right )-\bar{z}\left ( 2 \right ))/T-(\bar{z}\left ( 2 \right )-\bar{z}\left ( 1 \right ))/T)/T\\ \end{bmatrix}$

对应的 $k=3$ 时刻的协方差矩阵计算如下：

$P_{initial}(k)=\begin{bmatrix} P_{11}(k) & P_{12}(k) &P_{13}(k) \\ P_{12}(k) & P_{22}(k) &P_{23}(k)\\ P_{13}(k) & P_{23}(k) &P_{33}(k)\end{bmatrix} =C(k)*R\left ( k \right )*{C}'(k)$

$P(2)=\begin{bmatrix} P_{div}^{11} & P_{div}^{12} & P_{div}^{13}\\ P_{div}^{12} & P_{div}^{22} & P_{div}^{23}\\ P_{div}^{13} & P_{div}^{23}& P_{div}^{33} \end{bmatrix}$

其中， $R\left ( k \right )$ 为量测协方差矩阵。 $P_{div}$ 为分块矩阵，对应计算公式如下：

$P_{div}^{ij}=\begin{bmatrix} P_{ij}(3) & P_{ij}(3) /T &P_{ij}(3) /T^{2} \\ P_{ij}(3) /T & (P_{ij}(3)+ P_{ij}(2))/T^{2} & (P_{ij}(3)+2* P_{ij}(2))/T^{3} \\ P_{ij}(3) /T^{2} & (P_{ij}(3)+2* P_{ij}(2))/T^{3} & (P_{ij}(3)+4* P_{ij}(2)+P_{ij}(1))/T^{4} \end{bmatrix}$

其中， $i,j=1,2,3$ 。对应仿真代码如下：


n_sim = 100; %仿真时间
Ts = 1; %仿真时间间隔
nz = 3; %量测维数
x_start = [10, 2, 0, 30, -1, 0, 50, 2, 0]'; %目标初始时刻状态
nf = 0.001;
np = 3; %每一位的状态变量数：CV为2；CA为3
nd = 3; %空间维数：二维or三维
nx = np*nd; %状态维数
RMSE = zeros(n_sim,1);
Montrkalo = 100;
RMSE_sum = 0;
for k = 1:Montrkalo
    %目标真实轨迹及其量测
    [F,Q]= state_tran(Ts,nd,nf,np);
    x_real = zeros(nx,n_sim);
    x_real(:,1) = x_start;
    for i = 2:n_sim
        x_real(:,i) = F*x_real(:,i-1) + Q*randn(nx,1);
    end

    z_real = zeros(nz,n_sim);
    R = diag([1,0.0000001,0.0000001]);
    for i = 1:n_sim
        z_real(:,i) = h_calculate(x_real(:,i),np,nd)+R*randn(nz,1);
    end


    %滤波初始化
    x_filter = zeros(nx,n_sim);
    P = cell(n_sim,1);
    P(:) = {zeros(nx,nx)};

    n_initial = 3; %开始时刻
    rou = zeros(n_initial,1); theat = zeros(n_initial,1); epsion = zeros(n_initial,1);
    x = zeros(n_initial,1); y = zeros(n_initial,1); z = zeros(n_initial,1);

    R_trans = cell(n_initial,1);
    R_trans(:) = {zeros(nz,nz)};
    for i =1:n_initial
        rou(i) = z_real(1,i);
        theat(i) = z_real(2,i);
        epsion(i) = z_real(3,i);
        x(i) = rou(i)*cos(theat(i))*cos(epsion(i));%量测转换得到的
        y(i) = rou(i)*sin(theat(i))*cos(epsion(i));
        z(i) = rou(i)*sin(epsion(i));

        A = [cos(theat(i))*cos(epsion(i)),-rou(i)*sin(theat(i))*cos(epsion(i)),-rou(i)*cos(theat(i))*sin(epsion(i));
            sin(theat(i))*cos(epsion(i)),rou(i)*cos(theat(i))*cos(epsion(i)),-rou(i)*sin(theat(i))*sin(epsion(i));
            sin(epsion(i)), 0 , rou(i)*cos(theat(i))];
        R_trans{i} = A*R*A';
    end

    x_filter(:,n_initial) = [x(n_initial),(x(n_initial)-x(n_initial-1))/Ts,((x(n_initial)-x(n_initial-1))/Ts-(x(n_initial)-x(n_initial-1))/Ts)/Ts,...
        y(n_initial),(y(n_initial)-y(n_initial-1))/Ts,((y(n_initial)-y(n_initial-1))/Ts-(y(n_initial)-y(n_initial-1))/Ts)/Ts,...
        z(n_initial),(z(n_initial)-z(n_initial-1))/Ts,((z(n_initial)-z(n_initial-1))/Ts-(z(n_initial)-z(n_initial-1))/Ts)/Ts]';

    RMSE(n_initial) = sqrt((x_filter(1,n_initial)-x_real(1,n_initial))^2 + (x_filter(4,n_initial)-x_real(4,n_initial))^2+(x_filter(7,n_initial)-x_real(7,n_initial))^2);
    P_initial = cell(n_initial,n_initial);
    P_initial(:) = {zeros(nz,nz)};

    for i = 1:n_initial
        for j = 1:n_initial
            P_initial{i,j} = [R_trans{3}(i,j), R_trans{3}(i,j)/Ts, R_trans{3}(i,j)/(Ts^2);
                R_trans{3}(i,j)/Ts, (R_trans{3}(i,j)+R_trans{2}(i,j))/(Ts^2), (R_trans{3}(i,j)+2*R_trans{2}(i,j))/(Ts^3);
                R_trans{3}(i,j)/(Ts^2), (R_trans{3}(i,j)+2*R_trans{2}(i,j))/(Ts^3), (R_trans{3}(i,j)+4*R_trans{2}(i,j)+R_trans{1}(i,j))/(Ts^4) ];
        end
    end

    P{n_initial} = cell2mat(P_initial);

    for i = n_initial+1:n_sim
        [x_filter(:,i),P{i}] = UKF(x_filter(:,i-1),z_real(:,i),P{i-1},R,F,Q,np,nd);
        RMSE(i) = sqrt((x_filter(1,i)-x_real(1,i))^2 + (x_filter(4,i)-x_real(4,i))^2 + (x_filter(7,i)-x_real(7,i))^2);
    end

    RMSE_sum = RMSE_sum+RMSE;
end
RMSE_final = RMSE_sum/Montrkalo;

figure;
plot3(x_filter(1,n_initial:end),x_filter(4,n_initial:end),x_filter(7,n_initial:end),'r',LineWidth=1);
hold on;
grid on;
plot3(x_real(1,:),x_real(4,:),x_real(7,:),'b--',LineWidth=1);
xlabel('x');
ylabel('y');
zlabel('z');
legend("滤波轨迹","真实轨迹")

figure;
plot(n_initial:n_sim,RMSE_final(n_initial:end),'b--',LineWidth=1);
xlabel('RMSE');
ylabel('Time');