1. 理论
1.1 极大似然估计一般理论
n
n
n个随机观测值(
z
1
,
z
2
.
.
.
z
n
z_1,z_2...z_n
z1,z2...zn)的似然函数表示为:
p
(
z
∣
Θ
)
=
p
(
z
1
∣
Θ
)
⋅
p
(
z
2
∣
Θ
)
⋯
p
(
z
N
∣
Θ
)
=
∏
k
=
1
N
p
(
z
k
∣
Θ
)
(1)
\begin{aligned} p(z \mid \Theta) &=p\left(z_{1} \mid \Theta\right) \cdot p\left(z_{2} \mid \Theta\right) \cdots p\left(z_{N} \mid \Theta\right) \\ &=\prod_{k=1}^{N} p\left(z_{k} \mid \Theta\right) \end{aligned} \tag{1}
p(z∣Θ)=p(z1∣Θ)⋅p(z2∣Θ)⋯p(zN∣Θ)=k=1∏Np(zk∣Θ)(1)
因此可以得到极大似然估计为:
Θ
^
M
L
=
arg
{
min
Θ
ln
p
(
z
∣
Θ
)
}
(2)
\widehat{\Theta}_{\mathrm{ML}}=\arg \left\{\min _{\Theta} \ln p(z \mid \Theta)\right\}\tag{2}
Θ
ML=arg{Θminlnp(z∣Θ)}(2)
上式表示似然函数取得最小值时参数的值
Θ
^
M
L
\widehat{\Theta}_{\mathrm{ML}}
Θ
ML,取对数是为了方便计算,为了取得最小值,采用数值方法求导,得到以下公式:
∂
ln
p
(
z
∣
Θ
)
∂
Θ
=
0
(3)
\frac{\partial \ln p(z \mid \Theta)}{\partial \Theta}=0\tag{3}
∂Θ∂lnp(z∣Θ)=0(3)
∂ ln p ( z ∣ Θ 1 ) ∂ Θ ≈ ∂ ln p ( z ∣ Θ 0 ) ∂ Θ + ∂ 2 ln p ( z ∣ Θ 0 ) ∂ Θ 2 Δ Θ (4) \frac{\partial \ln p\left(z \mid \Theta_{1}\right)}{\partial \Theta} \approx \frac{\partial \ln p\left(z \mid \Theta_{0}\right)}{\partial \Theta}+\frac{\partial^{2} \ln p\left(z \mid \Theta_{0}\right)}{\partial \Theta^{2}} \Delta \Theta \tag{4} ∂Θ∂lnp(z∣Θ1)≈∂Θ∂lnp(z∣Θ0)+∂Θ2∂2lnp(z∣Θ0)ΔΘ(4)
∂ 2 ln p ( z ∣ Θ 0 ) ∂ Θ 2 Δ Θ = − ∂ ln p ( z ∣ Θ 0 ) ∂ Θ (5) \frac{\partial^{2} \ln p\left(z \mid \Theta_{0}\right)}{\partial \Theta^{2}} \Delta \Theta=-\frac{\partial \ln p\left(z \mid \Theta_{0}\right)}{\partial \Theta} \tag{5} ∂Θ2∂2lnp(z∣Θ0)ΔΘ=−∂Θ∂lnp(z∣Θ0)(5)
假设观测值存在误差,并且服从正态分布,那么在存在误差时候最大似然函数就可以表示为:
p
(
z
(
t
1
)
,
…
,
z
(
t
N
)
∣
Θ
,
R
)
=
∏
k
=
1
N
p
(
z
(
t
k
)
∣
Θ
,
R
)
=
{
(
2
π
)
n
y
∣
R
∣
}
−
N
/
2
exp
[
−
1
2
∑
k
=
1
N
[
z
(
t
k
)
−
y
(
t
k
)
]
T
R
−
1
[
z
(
t
k
)
−
y
(
t
k
)
]
]
(6)
\begin{aligned} &p\left(z\left(t_{1}\right), \ldots, z\left(t_{N}\right) \mid \Theta, R\right)=\prod_{k=1}^{N} p\left(z\left(t_{k}\right) \mid \Theta, R\right) \\ &\quad=\left\{(2 \pi)^{n_{y}}|R|\right\}^{-N / 2} \exp \left[-\frac{1}{2} \sum_{k=1}^{N}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]^{T} R^{-1}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]\right] \end{aligned}\tag{6}
p(z(t1),…,z(tN)∣Θ,R)=k=1∏Np(z(tk)∣Θ,R)={(2π)ny∣R∣}−N/2exp[−21k=1∑N[z(tk)−y(tk)]TR−1[z(tk)−y(tk)]](6)
取对数之后可以得到:
L
(
z
∣
Θ
,
R
)
=
1
2
∑
k
=
1
N
[
z
(
t
k
)
−
y
(
t
k
)
]
T
R
−
1
[
z
(
t
k
)
−
y
(
t
k
)
]
+
N
2
ln
(
det
(
R
)
)
+
N
n
y
2
ln
(
2
π
)
(7)
\begin{aligned} L(z \mid \Theta, R)=& \frac{1}{2} \sum_{k=1}^{N}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]^{T} R^{-1}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right] \\ &+\frac{N}{2} \ln (\operatorname{det}(R))+\frac{N n_{y}}{2} \ln (2 \pi) \end{aligned}\tag{7}
L(z∣Θ,R)=21k=1∑N[z(tk)−y(tk)]TR−1[z(tk)−y(tk)]+2Nln(det(R))+2Nnyln(2π)(7)
1.2 极大似然估计应用在飞行器参数估计
一般飞行器运动方程可以表示为:
x
˙
(
t
)
=
f
[
x
(
t
)
,
u
(
t
)
,
β
]
,
x
(
t
0
)
=
x
0
y
(
t
)
=
g
[
x
(
t
)
,
u
(
t
)
,
β
]
z
(
t
k
)
=
y
(
t
k
)
+
G
v
(
t
k
)
(8)
\begin{aligned} &\dot{x}(t)=f[x(t), u(t), \beta], \quad x\left(t_{0}\right)=x_{0} \\ &y(t)=g[x(t), u(t), \beta] \\ &z\left(t_{k}\right)=y\left(t_{k}\right)+G v\left(t_{k}\right) \end{aligned}\tag{8}
x˙(t)=f[x(t),u(t),β],x(t0)=x0y(t)=g[x(t),u(t),β]z(tk)=y(tk)+Gv(tk)(8)
带入上述公式即可,流程图如下:
L
(
z
∣
Θ
,
R
)
=
1
2
∑
k
=
1
N
[
z
(
t
k
)
−
y
(
t
k
)
]
T
R
−
1
[
z
(
t
k
)
−
y
(
t
k
)
]
+
N
2
ln
(
det
(
R
)
)
+
N
n
y
2
ln
(
2
π
)
(9)
\begin{aligned}L(z \mid \Theta, R)=& \frac{1}{2} \sum_{k=1}^{N}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]^{T} R^{-1}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right] \\&+\frac{N}{2} \ln (\operatorname{det}(R))+\frac{N n_{y}}{2} \ln (2 \pi)\end{aligned}\tag{9}
L(z∣Θ,R)=21k=1∑N[z(tk)−y(tk)]TR−1[z(tk)−y(tk)]+2Nln(det(R))+2Nnyln(2π)(9)
虽然公式中没有包含带估计参数
Θ
\Theta
Θ但是变量
y
y
y是通过公式计算得到,内部就包含了带估计参数。
初始误差矩阵通常由下式获得:
R
=
1
N
∑
k
=
1
N
[
z
(
t
k
)
−
y
(
t
k
)
]
[
z
(
t
k
)
−
y
(
t
k
)
]
T
(10)
R=\frac{1}{N} \sum_{k=1}^{N}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]^{T}\tag{10}
R=N1k=1∑N[z(tk)−y(tk)][z(tk)−y(tk)]T(10)
1.3 拟线性化求解带估计参数
根据上文所述,为了获得似然函数的最小值,需要求偏导,下面就讲解如何求偏导。
对似然函数求一阶导:
∂
J
∂
Θ
=
−
∑
k
=
1
N
[
∂
y
(
t
k
)
∂
Θ
]
T
R
−
1
[
z
(
t
k
)
−
y
(
t
k
)
]
=
0
(11)
\frac{\partial J}{\partial \Theta}=-\sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]=0\tag{11}
∂Θ∂J=−k=1∑N[∂Θ∂y(tk)]TR−1[z(tk)−y(tk)]=0(11)
采用拟线性化方法求解:
y
(
Θ
)
=
y
(
Θ
0
+
Δ
Θ
)
≈
y
(
Θ
0
)
+
∂
y
∂
Θ
Δ
Θ
(12)
y(\Theta)=y\left(\Theta_{0}+\Delta \Theta\right) \approx y\left(\Theta_{0}\right)+\frac{\partial y}{\partial \Theta} \Delta \Theta \tag{12}
y(Θ)=y(Θ0+ΔΘ)≈y(Θ0)+∂Θ∂yΔΘ(12)
带入方程得到
∂
J
∂
Θ
=
−
∑
k
=
1
N
[
∂
y
(
t
k
)
∂
Θ
]
T
R
−
1
[
z
(
t
k
)
−
{
y
(
t
k
)
+
∂
y
(
t
k
)
∂
Θ
Δ
Θ
}
]
=
0
(13)
\frac{\partial J}{\partial \Theta}=-\sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[z\left(t_{k}\right)-\left\{y\left(t_{k}\right)+\frac{\partial y\left(t_{k}\right)}{\partial \Theta} \Delta \Theta\right\}\right]=0\tag{13}
∂Θ∂J=−k=1∑N[∂Θ∂y(tk)]TR−1[z(tk)−{y(tk)+∂Θ∂y(tk)ΔΘ}]=0(13)
整理一下方程可得:
∑
k
=
1
N
[
∂
y
(
t
k
)
∂
Θ
]
T
R
−
1
[
z
(
t
k
)
−
y
(
t
k
)
]
−
∑
k
=
1
N
[
∂
y
(
t
k
)
∂
Θ
]
T
R
−
1
[
∂
y
(
t
k
)
∂
Θ
]
Δ
Θ
=
0
(14)
\sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]-\sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right] \Delta \Theta=0\tag{14}
k=1∑N[∂Θ∂y(tk)]TR−1[z(tk)−y(tk)]−k=1∑N[∂Θ∂y(tk)]TR−1[∂Θ∂y(tk)]ΔΘ=0(14)
∑ k = 1 N [ ∂ y ( t k ) ∂ Θ ] T R − 1 [ ∂ y ( t k ) ∂ Θ ] Δ Θ = ∑ k = 1 N [ ∂ y ( t k ) ∂ Θ ] T R − 1 [ z ( t k ) − y ( t k ) ] (15) \sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right] \Delta \Theta=\sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right]\tag{15} k=1∑N[∂Θ∂y(tk)]TR−1[∂Θ∂y(tk)]ΔΘ=k=1∑N[∂Θ∂y(tk)]TR−1[z(tk)−y(tk)](15)
总结得到:
Θ
i
+
1
=
Θ
i
+
Δ
Θ
and
F
Δ
Θ
=
−
G
(16)
\Theta_{i+1}=\Theta_{i}+\Delta \Theta \quad \text { and } \quad \mathcal{F} \Delta \Theta=-\mathcal{G}\tag{16}
Θi+1=Θi+ΔΘ and FΔΘ=−G(16)
F
=
∑
k
=
1
N
[
∂
y
(
t
k
)
∂
Θ
]
T
R
−
1
[
∂
y
(
t
k
)
∂
Θ
]
G
=
−
∑
k
=
1
N
[
∂
y
(
t
k
)
∂
Θ
]
T
R
−
1
[
z
(
t
k
)
−
y
(
t
k
)
]
(17)
\begin{aligned} \mathcal{F} &=\sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right] \\ \mathcal{G} &=-\sum_{k=1}^{N}\left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]^{T} R^{-1}\left[z\left(t_{k}\right)-y\left(t_{k}\right)\right] \end{aligned}\tag{17}
FG=k=1∑N[∂Θ∂y(tk)]TR−1[∂Θ∂y(tk)]=−k=1∑N[∂Θ∂y(tk)]TR−1[z(tk)−y(tk)](17)
这样只要获得矩阵
F
和
G
\mathcal{F}和\mathcal{G}
F和G,那么由公式(16)就能够获得带估计参数的偏差量
这样问题就总结到如何求解变量
∂
y
(
t
k
)
∂
Θ
\frac{\partial y\left(t_{k}\right)}{\partial \Theta}
∂Θ∂y(tk),也可以通过数值解法:
[
∂
y
(
t
k
)
∂
Θ
]
i
j
≈
y
i
p
(
t
k
)
−
y
i
(
t
k
)
δ
Θ
j
;
i
=
1
,
…
,
n
y
;
j
=
1
,
…
,
n
q
≈
g
i
(
x
p
(
t
k
)
,
u
(
t
k
)
,
Θ
+
δ
Θ
j
e
j
)
−
g
i
(
x
(
t
k
)
,
u
(
t
k
)
,
Θ
)
δ
Θ
j
(18)
\begin{aligned} \left[\frac{\partial y\left(t_{k}\right)}{\partial \Theta}\right]_{i j} & \approx \frac{y_{i}^{p}\left(t_{k}\right)-y_{i}\left(t_{k}\right)}{\delta \Theta_{j}} ; i=1, \ldots, n_{y} ; \quad j=1, \ldots, n_{q} \\ & \approx \frac{g_{i}\left(x^{p}\left(t_{k}\right), u\left(t_{k}\right), \Theta+\delta \Theta_{j} e^{j}\right)-g_{i}\left(x\left(t_{k}\right), u\left(t_{k}\right), \Theta\right)}{\delta \Theta_{j}} \end{aligned}\tag{18}
[∂Θ∂y(tk)]ij≈δΘjyip(tk)−yi(tk);i=1,…,ny;j=1,…,nq≈δΘjgi(xp(tk),u(tk),Θ+δΘjej)−gi(x(tk),u(tk),Θ)(18)
这样就完成了最后的估计,反复跌倒使得误差函数/损失函数最小即可
2. 程序实现
结合上面的推导并参考相关程序。
2.1 飞行器运动的状态方程
- 设置状态方程和观测方程
state_eq = 'xdot_TC01_attas_lat'; % function for state equations
obser_eq = 'obs_TC01_attas_lat'; % function for observation equations
- 在函数costfun_oem中计算误差/损失
y = feval(obser_eq, ts, x, u1, param, bXparV); % compute output variables, Eq. (4.11)
Y(kk,:) = y';
delta = Z(kk,:) - Y(kk,:);
delta = delta(:);
R = R + delta * delta';
将状态量带入方程获得估计的输出,与实际输出相比获得总的误差损失 R R R
-
在函数gradFG中计算 F , G F,G F,G矩阵
根据公式(18)计算H矩阵
% Compute outputs, Eq. (4.11), for pertubed parameters ys = feval(obser_eq, ts, xp, u1, paramp, bXparV); q1 = ys' - Y(kk,:); H(:,iPar) = q1'/par_del_iPar; % response gradients, Eq. (4.33)
其中paramp是摄动之后的参数,即公式(18)中的 δ Θ j \delta \Theta_{j} δΘj
接着就可以计算矩阵 F , G F,G F,G
zmy = Z(kk,:) - Y(kk,:); % response error zmy = zmy(:); G = G - H' * RI * zmy; % gradient vector, Eq. (4.30) F = F + H' * RI * H; % information matrix, Eq. (4.30)
-
更新估计量
在函数improv.m中
delPar = -uptF \ (uptF'\G); % Eq. (4.29); Delta-Theta
根据公式(16),这里的delpar就是参数偏差,然后在函数parUpdate里面对估计量进行修改
for ip=1:Nparam, if parFlag(ip) > 0, iPar = iPar + 1; param(ip) = param(ip) + delPar(iPar); end end % Initial conditons X0 iPar = 0; for i2=1:Nzi, for i1=1:Nx, if (parFlagX0(i1,i2) > 0), iPar = iPar + 1; x0(i1,i2) = x0(i1,i2) + delPar(NparID+iPar); end end end % bias parameters bXpar iPar = 0; for i2=1:Nzi, for i1=1:NbX, if (parFlagBX(i1,i2) > 0), iPar = iPar + 1; bXpar(i1,i2) = bXpar(i1,i2) + delPar(NparID+NX0ID+iPar); end end end % Build up the combined paramX0BX from updated param, x0, and bXpar paramX0BX = param; if ~isempty(x0), for kzi=1:Nzi, paramX0BX = [paramX0BX; x0(:,kzi)]; end end if ~isempty(bXpar), for kzi=1:Nzi, paramX0BX = [paramX0BX; bXpar(:,kzi)]; end end
这段代码用于更新所有参数,分为以下几步:
- 更新估计参数
- 更新初始状态量
- 更新参数偏差
- 将所有的量组合到一起写到参数paramX0BX里面