一、基本思想
系统动力学模型(状态模型或状态方程):
其中,为系统状态变量;为系统模型噪声,通常假设为零均值高斯白噪声。
系统观测模型(观测方程)
其中,为系统观测量,为系统观测噪声,也通常假设为零均值高斯白噪声。
假设已获得系统在时刻观测量,就可以估计时刻的系统状态。假设初始估计值为,根据系统微分方程,得到的状态变量,然后根据观测量与状态变量的关系,得到该估计的系统状态方程,得到估计观测量的值
残差:实际观测量与估计观测量之间偏差,即.
最小二乘思想:通过最优估计方法,使系统在观测区间内的残差平方和达到最小。定义使残差平方和最小的估计指标为:
令:
则估计指标可表示为:
若定义系统的最小二乘估计为,可使估计指标达到全局极小值,即满足:
但通常通常是隐性的函数关系,为高度非线性方程,难于求解
例如:假设在原点o处有一部雷达,水平方向距离雷达处有一枚炸弹,从高度以初速度自由下落,雷达以20Hz的采样频率,输出对该炸弹的观测量,该系统的状态方程和观测方程为
系统状态向量:
系统动力学模型:
系统观测模型:
目的:通过观测量,估计炸弹的状态向量
相互独立:,零均值高斯白噪声:
零均值:
高斯:概率密度函数是正态函数
白噪声:,
根据自由落体运动规律
代入观测方程中得到的形式非常复杂。
根据系统观测方程:
求解系统观测矩阵:
根据先验知识,得到的状态估计量为,代入系统观测矩阵
设预估状态和真实状态之差为
观测方程及其线性化后
根据给观测的数据和先验状态下得到的观测值
得到以下等式
根据系统状态方程:
得到状态转移矩阵为
假设第k-1次迭代后,自由落体初始状态估计为,则第k次迭代的状态微分修正量,满足微分修正条件方程:
那么根据最小二乘原理,求解出,然后将求出的值对求出的先验估计进行修正
然后采用同样的方式进行上述操作,进行新一轮迭代,当修正量足够小时,即认为该值为估计值。
二、案例实践
1、首先假设真实情况下目标的初始高度为,观测位置在水平位置的分量为5m,观测时刻为,步长为0.2s,开始观测时间为0.2s,根据系统状态转移方程,得到其观测得到的值,代码如下
g=9.8; %重力加速度
h0=20; %假设其真实高度为20m
l=5; %水平位置为5m
v0=0; %真实初始速度为0m/s
t=0:0.2:2; %得到观测时间
X=zeros(2,length(t));
rou=zeros(1,length(t));
E=zeros(1,length(t));
%% 写成该系统的动力学方程
X0=[h0;v0];
for i=1:length(t)
phi=[1 t(i);0 1]; %状态转移矩阵
const=[-g*(t(i)^2)/2;-g*t(i)];
X(:,i)=phi*X0+const; %状态转移方程
rou(i)=sqrt(X(1,i)^2+l^2)+10e-4*randn(1); %得到真实观测的距离
E(i)=atan(X(1,i)/l)+10e-4*randn(1); %得到真实观测的俯仰角
end
2、设计先验估计值为,根据先验估计值,代入状态转移方程和观测方程,得到其观测值与估计观测值的差,代码如下:
hat_X0=[25;0];hat_X=zeros(2,length(t));
hat_rou=zeros(1,length(t)-1);
hat_E=zeros(1,length(t)-1);
zz=zeros(1,2*(length(t)-1));
for i=2:length(t)-1
phi=[1 t(i);0 1];
const=[-g*(t(i)^2)/2;-g*t(i)];
hat_X(:,i)=phi*hat_X0+const;%% 根据先验值得到的每个时刻的状态值
hat_rou(i)=sqrt(hat_X(1,i)^2+l^2)+10e-4*randn(1);
hat_E(i)=atan(hat_X(1,i)/l)+10e-4*randn(1);
zz((i-1)*2-1)=rou(i)-hat_rou(i);
zz((i-1)*2)=E(i)-hat_E(i);
end
z=zz';%得到最小二乘法中对应的Z项
3、根据上文介绍的线性化方法,求出每个时刻的对应的H矩阵,例如:状态量,则,其对应的雅可比矩阵
该时刻的状态转移方程为
通过两个式子相乘得到对应的系数矩阵,根据最小二乘法原理,使残差最小,得到估计值
H=zeros(2*(length(t)-1),2);
for m=2:length(t)
X1=hat_X(:,m);
Jac=[X1(1)/sqrt(X1(1)^2+l^2) 0;l/(X1(1)^2+l^2) 0];
Phi=[1 t(m);0 1];
h=Jac*Phi;
H((m-1)*2-1,:)=h(1,:);
H((m-1)*2,:)=h(2,:);
end
Hat_Delta_X0=inv(H'*H)*(H'*z)
求解出的结果为
4、将求出的修正值代入先验估计中,重新进行上一步操作,得到的第二次修正结果
,进行第三次迭代,得到第三次的修正结果为,修正量较小,停止迭代,得到最终条件为。整个问题的代码在下面给出
clc;clear
%% 假设目标从20m高度往下扔
%% 距离横向长度为5m,观测10次数据 采样间隔为1s
g=9.8;
h0=20;
l=5;
v0=0;
t=0:0.2:2;
X=zeros(2,length(t));
rou=zeros(1,length(t));
E=zeros(1,length(t));
%% 写成该系统的动力学方程
X0=[h0;v0];
for i=1:length(t)
phi=[1 t(i);0 1];
const=[-g*(t(i)^2)/2;-g*t(i)];
X(:,i)=phi*X0+const;
rou(i)=sqrt(X(1,i)^2+l^2)+10e-4*randn(1);
E(i)=atan(X(1,i)/l)+10e-4*randn(1);
end
%% 估计X0的初始位置
Hat_Delta_X0=[10;10];hat_X0=[25;0];
while abs(Hat_Delta_X0(1))>0.01 && abs(Hat_Delta_X0(2))>0.01
hat_X=zeros(2,length(t));
hat_rou=zeros(1,length(t)-1);
hat_E=zeros(1,length(t)-1);
zz=zeros(1,2*(length(t)-1));
for i=2:length(t)-1
phi=[1 t(i);0 1];
const=[-g*(t(i)^2)/2;-g*t(i)];
hat_X(:,i)=phi*hat_X0+const;%% 根据先验值得到的每个时刻的状态值
hat_rou(i)=sqrt(hat_X(1,i)^2+l^2)+10e-4*randn(1);
hat_E(i)=atan(hat_X(1,i)/l)+10e-4*randn(1);
zz((i-1)*2-1)=rou(i)-hat_rou(i);
zz((i-1)*2)=E(i)-hat_E(i);
end
%对于h1时刻的状态
z=zz';H=zeros(2*(length(t)-1),2);
for m=2:length(t)
X1=hat_X(:,m);
Jac=[X1(1)/sqrt(X1(1)^2+l^2) 0;l/(X1(1)^2+l^2) 0];
Phi=[1 t(m);0 1];
h=Jac*Phi;
H((m-1)*2-1,:)=h(1,:);
H((m-1)*2,:)=h(2,:);
end
Hat_Delta_X0=inv(H'*H)*(H'*z);
hat_X0=hat_X0+Hat_Delta_X0
end