Kalman滤波学习笔记二《数学基础:向量与矩阵》

Kalman滤波学习笔记二《数学基础:向量与矩阵》

学习笔记参考:《Kalman滤波基础及MATLAB仿真》北京航空航天大学出版社——王可东编著
注:本篇笔记根据博主个人数学的掌握情况整理

一、向量
1、内积:同维数的两个向量可以求内积,结果为对应元素的乘积之和,为一标量。 x T y = y T x = ∑ i = 1 n x i y i {\boldsymbol x}^{T}{\boldsymbol y}={\boldsymbol y}^{T}{\boldsymbol x}=\sum_{i=1}^n x_iy_i \quad xTy=yTx=i=1nxiyi 如果 x T y = 0 {\boldsymbol x}^{T}{\boldsymbol y}=0 xTy=0,则 x x x y y y 正交; x x x 的长度: ∣ x ∣ = x T x |{\boldsymbol x}|=\sqrt{{\boldsymbol x}^{T}{\boldsymbol x}} \quad x=xTx
2、外积:同维数的两个向量可以求外积,结果为一方阵。
x y T = ( x 1 y 1 x 1 y 2 ⋯ x 1 y n x 2 y 1 x 2 y 2 ⋯ x 2 y n ⋮ ⋮ ⋱ ⋮ x n y 1 x n y 2 ⋯ x n y n ) {\boldsymbol x}{\boldsymbol y}^{T} = \begin{pmatrix} x_1y_1 & x_1y_2 & \cdots & x_1y_n \\ x_2y_1 & x_2y_2 & \cdots & x_2y_n \\ \vdots & \vdots & \ddots & \vdots \\ x_ny_1 & x_ny_2 & \cdots & x_ny_n \\ \end{pmatrix} xyT=x1y1x2y1xny1x1y2x2y2xny2x1ynx2ynxnyn x {\boldsymbol x} x 的扩散矩阵为:
x x T = ( x 1 2 x 1 x 2 ⋯ x 1 x n x 2 x 1 x 2 2 ⋯ x 2 x n ⋮ ⋮ ⋱ ⋮ x n x 1 x n x 2 ⋯ x n 2 ) {\boldsymbol x}{\boldsymbol x}^{T} = \begin{pmatrix} x_1^{2} & x_1x_2 & \cdots & x_1x_n \\ x_2x_1 & x_2^{2} & \cdots & x_2x_n \\ \vdots & \vdots & \ddots & \vdots \\ x_nx_1 & x_nx_2 & \cdots & x_n^{2} \\ \end{pmatrix} xxT=x12x2x1xnx1x1x2x22xnx2x1xnx2xnxn2 3、导数:向量的导数为其每个元素求导。
4、积分:向量的积分为其每个元素积分。

二、矩阵
1、导数和积分:矩阵的导数和积分为对其每个元素求导数和积分。
2、单位矩阵:
I = ( 1 0 ⋯ 0 0 1 ⋯ 0 ⋮ ⋮ ⋱ ⋮ 0 0 ⋯ 1 ) = [ δ i j ] {\boldsymbol I} = \begin{pmatrix} 1 & 0 & \cdots & 0 \\ 0 & 1 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & 1 \\ \end{pmatrix}=[\delta_{ij}] I=100010001=[δij] 其中, δ i j \delta_{ij} δij 为Kronecker δ \delta δ 函数,定义为:
δ i j = { 1 , i = j 0 , i ≠ j \delta_{ij} = \begin{cases} 1, & i=j \\[2ex] 0, & i\neq j \end{cases} δij=1,0,i=ji=j3、迹:方阵主对角线上元素的代数和称为该方阵的迹: t r ( A ) = ∑ i = 1 n a i i tr({\boldsymbol A})=\sum_{i=1}^n a_{ii} \quad tr(A)=i=1naii4、伪逆:对 A m × n {\boldsymbol A_{m \times n}} Am×n,m>n,且 ( A T A ) ({\boldsymbol A}^{T}{\boldsymbol A}) (ATA) 非奇异,则其伪逆定义为:
A # = ( A T A ) − 1 A T {\boldsymbol A}^{\#}=({\boldsymbol A}^{T}{\boldsymbol A})^{-1}{\boldsymbol A}^{T} A#=(ATA)1AT 若 m A # = A T ( A A T ) − 1 {\boldsymbol A}^{\#}={\boldsymbol A}^{T}({\boldsymbol A}{\boldsymbol A}^{T})^{-1} A#=AT(AAT)1 伪逆的运算性质有:
{ A A # A = A A # A A # = A # ( A # A ) T = A # A ( A A # ) T = A A # \left\{ \begin{array}{c} {\boldsymbol A}{\boldsymbol A}^{\#}{\boldsymbol A} ={\boldsymbol A} \\ {\boldsymbol A}^{\#}{\boldsymbol A}{\boldsymbol A}^{\#}={\boldsymbol A}^{\#} \\ ({\boldsymbol A}^{\#}{\boldsymbol A})^{T}={\boldsymbol A}^{\#}{\boldsymbol A} \\ ({\boldsymbol A}{\boldsymbol A}^{\#})^{T}={\boldsymbol A}{\boldsymbol A}^{\#} \end{array}\right. AA#A=AA#AA#=A#(A#A)T=A#A(AA#)T=AA# 5、方阵函数 λ \lambda λ 为方阵 A {\boldsymbol A} A 的特征值,特征方程为:
f ( λ ) = ∣ λ I − A ∣ = λ n + a 1 λ n − 1 + a 2 λ n − 2 + ⋯ + a n − 1 λ 1 + a n = 0 f(\lambda)=|\lambda{\boldsymbol I}-{\boldsymbol A}|={\lambda}^{n}+a_1{\lambda}^{n-1}+a_2{\lambda}^{n-2}+\cdots+a_{n-1}{\lambda}^{1}+a_n=0 f(λ)=λIA=λn+a1λn1+a2λn2++an1λ1+an=0 由Cayley公式知,将方阵 A {\boldsymbol A} A 代入特征方程有: f ( A ) = 0 f({\boldsymbol A})=0 f(A)=0
对方阵 A {\boldsymbol A} A 定义其对应的指数矩阵为:
e A = I + A + 1 2 ! A 2 + ⋯ e^{\boldsymbol A} = \boldsymbol I + \boldsymbol A + \frac {1} {2!}{\boldsymbol A}^2+\cdots eA=I+A+2!1A2+ 其中, A 2 = A A , A 3 = A A A , A k + 1 = ( A k ) A {\boldsymbol A}^2={\boldsymbol A}{\boldsymbol A},{\boldsymbol A}^3={\boldsymbol A}{\boldsymbol A}{\boldsymbol A},{\boldsymbol A}^{k+1}=({\boldsymbol A}^k){\boldsymbol A} A2=AA,A3=AAA,Ak+1=(Ak)A指数矩阵性质有:
① 若 A B = B A {\boldsymbol A}{\boldsymbol B}={\boldsymbol B}{\boldsymbol A} AB=BA ,则 e A + B = e A e B e^{{\boldsymbol A}+{\boldsymbol B}}=e^{{\boldsymbol A}}e^{{\boldsymbol B}} eA+B=eAeB
② 若 ∣ T ∣ ≠ 0 |{\boldsymbol T}| \neq 0 T=0 ,则 e T F T − 1 = T e F T − 1 e^{{\boldsymbol T}{\boldsymbol F}{\boldsymbol T}^{-1}}={\boldsymbol T}e^{{\boldsymbol F}}{\boldsymbol T}^{-1} eTFT1=TeFT1
∣ e F ∣ = e t r ( F ) |e^{{\boldsymbol F}}| = e^{tr({\boldsymbol F})} eF=etr(F)

三、向量—矩阵运算
1、二次型:设 A n × n {\boldsymbol A_{n \times n}} An×n 为一对称矩阵, x {\boldsymbol x} x 为一 n n n 维列向量,那么关于对称矩阵 A {\boldsymbol A} A 的二次型定义为: J = x T A x J={\boldsymbol x}^T{\boldsymbol A}{\boldsymbol x} J=xTAx A {\boldsymbol A} A 的特征值为 { λ i } ( i = 1 , 2 , ⋯   , n ) \{\lambda_i\}(i=1,2,\cdots,n) {λi}(i=1,2,,n),那么,存在正交矩阵 Q {\boldsymbol Q} Q,使得: J = x T A x = x ′ T A ′ x ′ = ∑ i = 1 n λ i x i ′ 2 J={\boldsymbol x}^T{\boldsymbol A}{\boldsymbol x}={\boldsymbol x'}^{T}{\boldsymbol A'}{\boldsymbol x'}=\sum_{i=1}^n \lambda_i{x'_i}^{2} \quad J=xTAx=xTAx=i=1nλixi2其中, x ′ = Q T x x'={\boldsymbol Q}^{T}{\boldsymbol x} x=QTx A ′ = Q T A Q = d i a g ( λ i ) {\boldsymbol A}'={\boldsymbol Q}^{T}{\boldsymbol A}{\boldsymbol Q}=diag(\lambda_i) A=QTAQ=diag(λi) d i a g ( ⋅ ) diag(\cdot) diag() 表示对角线上的元素为相应值,其它非对角线元素均为 0 0 0
2、定:对于对称方阵 A {\boldsymbol A} A,按照其二次型的结果,定义如下四种定:
① 正定: J = x T A x > 0 J={\boldsymbol x}^T{\boldsymbol A}{\boldsymbol x}>0 J=xTAx>0
② 负定: J = x T A x < 0 J={\boldsymbol x}^T{\boldsymbol A}{\boldsymbol x}<0 J=xTAx<0
③ 半正定: J = x T A x ≥ 0 J={\boldsymbol x}^T{\boldsymbol A}{\boldsymbol x} \geq 0 J=xTAx0
③ 半负定: J = x T A x ≤ 0 J={\boldsymbol x}^T{\boldsymbol A}{\boldsymbol x} \leq 0 J=xTAx0
对于物理可实现系统其对应的系数矩阵都是正定的与可逆也是等价的
3、范数
向量 x {\boldsymbol x} x 的范数 ∣ ∣ x ∣ ∣ ||{\boldsymbol x}|| x 需满足如下三个条件:
∣ ∣ k x ∣ ∣ = ∣ k ∣ ∣ ∣ x ∣ ∣ ||k {\boldsymbol x}||=|k| ||{\boldsymbol x}|| kx=kx,其中 k k k 为标量;
∣ ∣ x + y ∣ ∣ ≤ ∣ ∣ x ∣ ∣ + ∣ ∣ y ∣ ∣ ||{\boldsymbol x}+{\boldsymbol y}|| \leq ||{\boldsymbol x}||+||{\boldsymbol y}|| x+yx+y
③ 若 ∣ ∣ x ∣ ∣ = 0 ||{\boldsymbol x}||=0 x=0,则 x = 0 {\boldsymbol x}={\boldsymbol 0} x=0
显然,满足上述条件的范数有很多种,其中最常见的有:
E u c l i d Euclid Euclid 范数 (2 范数): ∣ ∣ x ∣ ∣ 2 = x T x ||{\boldsymbol x}||_2=\sqrt{{\boldsymbol x}^{T}{\boldsymbol x}} \quad x2=xTx M a n h a t t a n Manhattan Manhattan 范数 (1 范数): ∣ ∣ x ∣ ∣ 1 = ∑ i = 1 n ∣ x i ∣ ||{\boldsymbol x}||_1=\sum_{i=1}^n |x_i|\quad x1=i=1nxi③ 无穷范数: ∣ ∣ x ∣ ∣ ∞ = m a x ( ∣ x 1 ∣ , ∣ x 2 ∣ , ⋯   , ∣ x n ∣ ) ||{\boldsymbol x}||_\infty=max(|x_1|,|x_2|,\cdots,|x_n|) x=max(x1,x2,,xn) p p p 范数:设 p p p 为一不小于1的实数, p p p 范数定义为: ∣ ∣ x ∣ ∣ p = ( ∑ i = 1 n ∣ x i ∣ p ) 1 p ||{\boldsymbol x}||_p=(\sum_{i=1}^n |x_i|^{p})^{\frac {1} {p}} xp=(i=1nxip)p1类似地,也可以定义矩阵 A m × n {\boldsymbol A_{m \times n}} Am×n 的范数如下:
① 1 范数: ∣ ∣ A ∣ ∣ 1 = max ⁡ 1 ⩽ j ⩽ n ∑ i = 1 m ∣ a i j ∣ {||{\boldsymbol A}||}_1=\mathop {\max }\limits_{1\leqslant j \leqslant n} \sum_{i=1}^m |a_{ij}| A1=1jnmaxi=1maij② 无穷范数: ∣ ∣ A ∣ ∣ ∞ = max ⁡ 1 ⩽ i ⩽ m ∑ j = 1 n ∣ a i j ∣ {||{\boldsymbol A}||}_{\infty}=\mathop {\max }\limits_{1\leqslant i \leqslant m} \sum_{j=1}^n |a_{ij}| A=1immaxj=1naij③ 2 范数: ∣ ∣ A ∣ ∣ 2 = λ m a x ( A ∗ A ) {||{\boldsymbol A}||}_2=\sqrt{\lambda_{max}({\boldsymbol A}^{*}{\boldsymbol A}}) A2=λmax(AA )其中, A ∗ {\boldsymbol A}^{*} A A {\boldsymbol A} A复共轭转置矩阵 λ m a x \lambda_{max} λmax A ∗ A {\boldsymbol A}^{*}{\boldsymbol A} AA 的最大特征值。
F r o b e n i u s Frobenius Frobenius 范数: ∣ ∣ A ∣ ∣ F = ( ∑ i = 1 m ∑ j = 1 n ∣ a i j ∣ 2 ) 1 2 {||{\boldsymbol A}||}_F=(\sum_{i=1}^m \sum_{j=1}^n |a_{ij}|^{2})^{\frac {1} {2}} AF=(i=1mj=1naij2)21 4、梯度运算
标量函数对一向量求梯度
标量对一向量求梯度运算为该标量对向量的每个元素求梯度构成一向量,因而,梯度运算得到的向量与求梯度运算的向量同维数,即: ∂ z ∂ x = a \frac{{\rm \partial}z}{{\rm \partial}{\boldsymbol x}} ={\boldsymbol a} xz=a其中, a i = ∂ z ∂ x i a_i= \frac{{\rm \partial}z}{{\rm \partial}x_i} ai=xiz
内积
内积也为标量,其梯度运算结果为: ∂ ∂ x ( y T x ) = y \frac{{\rm \partial}}{{\rm \partial}{\boldsymbol x}}({\boldsymbol y}^{T}{\boldsymbol x})={\boldsymbol y} x(yTx)=y ∂ ∂ x ( x T y ) = y \frac{{\rm \partial}}{{\rm \partial}{\boldsymbol x}}({\boldsymbol x}^{T}{\boldsymbol y})={\boldsymbol y} x(xTy)=y标量函数对一向量的二阶梯度
标量对一向量求二阶梯度,结果为一方阵,即: ∂ 2 z ∂ x 2 = A \frac{{\rm \partial}^2z}{{\rm \partial}{\boldsymbol x^2}}={\boldsymbol A} x22z=A其中, a i j = ∂ 2 z ∂ x i ∂ x j a_{ij}=\frac{{\rm \partial}^2z}{{\rm \partial}{x_i}{\rm \partial}{x_j}} aij=xixj2z ∣ A ∣ |{\boldsymbol A}| A z z z H e s s i a n Hessian Hessian 矩阵行列式。
向量对向量求梯度
一向量对另一向量求梯度,为向量的每个元素对另一向量的每个元素求梯度结果为一矩阵,即: ∂ z T ∂ x = A \frac{{\rm \partial}{\boldsymbol z}^{T}}{{\rm \partial}{\boldsymbol x}}={\boldsymbol A} xzT=A其中, a i j = ∂ z j ∂ x i a_{ij}=\frac{{\rm \partial}z_j}{{\rm \partial}x_i} aij=xizj 。若 z {\boldsymbol z} z x {\boldsymbol x} x 同维,则 ∣ A ∣ |{\boldsymbol A}| A z {\boldsymbol z} z J a c o b i a n Jacobian Jacobian 矩阵行列式。
标量对矩阵求梯度
标量对矩阵求梯度,为标量对矩阵的每个元素求梯度结果为同维度矩阵,即: ∂ z ∂ A = B \frac{{\rm \partial}z}{{\rm \partial}{\boldsymbol A}}={\boldsymbol B} Az=B其中, b i j = ∂ z ∂ a i j b_{ij}=\frac{{\rm \partial}z}{{\rm \partial}a_{ij}} bij=aijz B {\boldsymbol B} B 矩阵又称为 H e s s i a n Hessian Hessian 矩阵。
对方阵 A {\boldsymbol A} A B {\boldsymbol B} B C {\boldsymbol C} C,有如下式子成立: ∂ ∂ A t r ( A ) = I \frac{{\rm \partial}}{{\rm \partial}{\boldsymbol A}}tr({\boldsymbol A})={\boldsymbol I} Atr(A)=I ∂ ∂ A t r ( B A C ) = B T C T \frac{{\rm \partial}}{{\rm \partial}{\boldsymbol A}}tr({\boldsymbol B}{\boldsymbol A}{\boldsymbol C})={\boldsymbol B}^{T}{\boldsymbol C}^{T} Atr(BAC)=BTCT ∂ ∂ A t r ( A B A T ) = A ( B T + B ) \frac{{\rm \partial}}{{\rm \partial}{\boldsymbol A}}tr({\boldsymbol A}{\boldsymbol B}{\boldsymbol A}^{T})={\boldsymbol A}({\boldsymbol B}^{T}+{\boldsymbol B}) Atr(ABAT)=A(BT+B) ∂ ∂ A t r ( e A ) = e A T \frac{{\rm \partial}}{{\rm \partial}{\boldsymbol A}}tr(e^{\boldsymbol A})=e^{{\boldsymbol A}^{T}} Atr(eA)=eAT ∂ ∂ A ∣ B A C ∣ = ∣ B A C ∣ ( A − 1 ) T \frac{{\rm \partial}}{{\rm \partial}{\boldsymbol A}}|{\boldsymbol B}{\boldsymbol A}{\boldsymbol C}|=|{\boldsymbol B}{\boldsymbol A}{\boldsymbol C}|({\boldsymbol A}^{-1})^{T} ABAC=BAC(A1)T二次型求梯度 ∂ x T A x ∂ x = ( A + A T ) x \frac{{\rm \partial}{\boldsymbol x}^{T}{\boldsymbol A}{\boldsymbol x}}{{\rm \partial}{\boldsymbol x}}=({\boldsymbol A}+{\boldsymbol A}^{T}){\boldsymbol x} xxTAx=(A+AT)x
四、最小二乘算法
1、例一:设一向量 x {\boldsymbol x} x,其对应的测量向量为 z {\boldsymbol z} z,且有 z = H x + v {\boldsymbol z}={\boldsymbol H}{\boldsymbol x}+{\boldsymbol v} z=Hx+v,其中 H {\boldsymbol H} H 为量测矩阵, v {\boldsymbol v} v 为量测噪声。最优准则设置为:测量值与其估计值的偏差平方和最小,试求基于 z {\boldsymbol z} z 关于 x {\boldsymbol x} x 的最优估计。
解:设 x {\boldsymbol x} x 的估计值为 x ^ \hat{\boldsymbol x} x^,其中量测噪声是无法估计的,那么测量值的估计为 z ^ = H x ^ \hat{\boldsymbol z}={\boldsymbol H}\hat{\boldsymbol x} z^=Hx^。按照估计准则有: J = ( z − z ^ ) T ( z − z ^ ) = ( z − H x ^ ) T ( z − H x ^ ) J=({\boldsymbol z}-\hat{\boldsymbol z})^{T}({\boldsymbol z}-\hat{\boldsymbol z})=({\boldsymbol z}-{\boldsymbol H}\hat{\boldsymbol x})^{T}({\boldsymbol z}-{\boldsymbol H}\hat{\boldsymbol x}) J=(zz^)T(zz^)=(zHx^)T(zHx^) J J J 是关于 x ^ \hat{\boldsymbol x} x^ 的函数,且为平方和的形式,因此,当: ∂ J ∂ x ^ = 0 \frac{{\rm \partial}J}{{\rm \partial}\hat{\boldsymbol x}}={\boldsymbol 0} x^J=0 时, J J J 取极小值,有: ∂ J ∂ x ^ = [ ∂ ( z − H x ^ ) ∂ x ^ ] T [ ∂ [ ( z − H x ^ ) T I ( z − H x ^ ) ] ∂ ( z − H x ^ ) ] = − H T ( I + I T ) ( z − H x ^ ) \frac{{\rm \partial}J}{{\rm \partial}\hat{\boldsymbol x}}=\left[ \frac{{\rm \partial}({\boldsymbol z}-{\boldsymbol H}\hat{\boldsymbol x})}{{\rm \partial}\hat{\boldsymbol x}}\right]^{T}\left[ \frac{{\rm \partial}\left[({\boldsymbol z}-{\boldsymbol H}\hat{\boldsymbol x})^{T}{\boldsymbol I}({\boldsymbol z}-{\boldsymbol H}\hat{\boldsymbol x})\right]}{{\rm \partial}({\boldsymbol z}-{\boldsymbol H}\hat{\boldsymbol x})}\right]=-{\boldsymbol H}^{T}({\boldsymbol I}+{\boldsymbol I}^{T})({\boldsymbol z}-{\boldsymbol H}\hat{\boldsymbol x}) x^J=[x^(zHx^)]T[(zHx^)[(zHx^)TI(zHx^)]]=HT(I+IT)(zHx^) = − 2 H T z + 2 H T H x ^ = 0 =-2{\boldsymbol H}^{T}{\boldsymbol z}+2{\boldsymbol H}^{T}{\boldsymbol H}\hat{\boldsymbol x}={\boldsymbol 0} =2HTz+2HTHx^=0 H T H {\boldsymbol H}^{T}{\boldsymbol H} HTH 可逆时,有: x ^ = ( H T H ) − 1 H T z \hat{\boldsymbol x}=({\boldsymbol H}^{T}{\boldsymbol H})^{-1}{\boldsymbol H}^{T}{\boldsymbol z} x^=(HTH)1HTz 上式就是基于 z {\boldsymbol z} z 测量值对 x {\boldsymbol x} x 在最小二乘意义上的最优估计结果
:上述求导过程运用到矩阵向量的链式求导法则。
2、例二:设一辆汽车在一条公路上匀速直线行驶,速度为 30 m / s 30m/s 30m/s。车上安装了BDS接收机,其测速误差是均值为 0 0 0、标准差为 0.1 m / s 0.1m/s 0.1m/s 的高斯噪声;同时,车载测速仪的测速误差是均值为 0 0 0、标准差为 0.5 m / s 0.5m/s 0.5m/s 的高斯噪声。试利用最小二乘算法估计该车辆 100 s 100s 100s 之内的速度,并与Kalman滤波学习笔记一第12条的结果进行对比
解:k时刻的测量量为: z k = [ z 1 , k z 2 , k ] = [ 1 1 ] x k + [ v 1 , k v 2 , k ] = H k x k + v k {\boldsymbol z}_k=\begin{bmatrix} z_{1,k} \\ z_{2,k} \\ \end{bmatrix}=\begin{bmatrix} 1 \\ 1 \\ \end{bmatrix} x_k+\begin{bmatrix} v_{1,k} \\ v_{2,k} \\ \end{bmatrix}={\boldsymbol H}_kx_k+{\boldsymbol v}_k zk=[z1,kz2,k]=[11]xk+[v1,kv2,k]=Hkxk+vk最小二乘算法是批处理算法,即累积从初始时刻到k时刻的所有测量值,对k时刻的状态进行估计,因此,k时刻利用的测量量为: z ˉ k = [ z 1 z 2 ⋮ z k ] = [ H 1 H 2 ⋮ H k ] x k + [ v 1 v 2 ⋮ v k ] = H ‾ k x k + v ˉ k \bar{\boldsymbol z}_k=\begin{bmatrix}{\boldsymbol z}_1 \\{\boldsymbol z}_2\\ \vdots\\ {\boldsymbol z}_k \\ \end{bmatrix}=\begin{bmatrix}{\boldsymbol H}_1 \\{\boldsymbol H}_2\\ \vdots\\ {\boldsymbol H}_k \\ \end{bmatrix} x_k +\begin{bmatrix}{\boldsymbol v}_1 \\{\boldsymbol v}_2\\ \vdots\\ {\boldsymbol v}_k \\ \end{bmatrix}=\overline{\boldsymbol H}_kx_k+\bar{\boldsymbol v}_k zˉk=z1z2zk=H1H2Hkxk+v1v2vk=Hkxk+vˉk x k {\boldsymbol x}_k xk 在最小二乘意义上的最优估计结果为: x ^ k = ( H ‾ k T H ‾ k ) − 1 H ‾ k T z ˉ k = 1 2 k ∑ i = 1 k ( z 1 , i + z 2 , i ) \hat{\boldsymbol x}_k=(\overline{\boldsymbol H}_k^{T}\overline{\boldsymbol H}_k)^{-1}\overline{\boldsymbol H}_k^{T}\bar{\boldsymbol z}_k=\frac {1} {2k}\sum_{i=1}^k (z_{1,i}+z_{2,i}) x^k=(HkTHk)1HkTzˉk=2k1i=1k(z1,i+z2,i)

Kalman滤波学习笔记二《数学基础:向量与矩阵》_第1张图片

MATLAB代码如下

clear all; close all;

N = 100;
x = 50 * ones(N,1);  %被估计对象
sigma1 = 0.5; sigma2 = 0.1;
v1 = sigma1 * randn(N,1);
v2 = sigma2 * randn(N,1);
z1 = x + v1;
z2 = x + v2;

w1 = sigma1^2/(sigma1^2 + sigma2^2);
w2 = sigma2^2/(sigma1^2 + sigma2^2);
x_est = w1 * z2 + w2 * z1;

x_est_ls_1 = [];
x_est_ls_2 = [];
sum_1 = 0;
sum_2 = 0;

for i = 1:N
    sum_1 = sum_1 + z1(i);
    sum_2 = sum_2 + z2(i);
    est_1 = sum_1/i;
    est_2 = (sum_1 + sum_2)/(2*i);
    x_est_ls_1 = [x_est_ls_1;est_1];
    x_est_ls_2 = [x_est_ls_2;est_2];
end
x_est_err = x_est-x;
x_est_ls_1_err = x_est_ls_1 - x;
x_est_ls_2_err = x_est_ls_2 - x;

plot(1:N,x_est_err,'k',1:N,x_est_ls_1_err,'k*-',1:N,x_est_ls_2_err,'ko-')
legend('最小方差估计误差','最小二乘估计误差(单测量)','最小二乘估计误差(双测量)');
xlabel('时间(s)');ylabel('速度误差(m/s)')

两个结论
① 为了提高估计精度,应尽可能地利用从初始时刻到当前时刻的所有测量值,实现对当前时刻的状态估计;
基于多个传感器的测量结果进行状态估计有利于提高估计精度。



课后习题
1、设矩阵 A = [ 1 2 3 4 ] {\boldsymbol A}=\begin{bmatrix} 1 & 2 \\ 3 & 4 \\ \end{bmatrix} A=[1324]
① 试求矩阵 A {\boldsymbol A} A 的特征值 λ 1 \lambda_1 λ1 λ 2 \lambda_2 λ2 以及 A 3 {\boldsymbol A}^3 A3 A 5 {\boldsymbol A}^5 A5
② 若 e A t = a I + b A e^{{\boldsymbol A}t}=a{\boldsymbol I}+b{\boldsymbol A} eAt=aI+bA,试证明 a = λ 1 e λ 2 t − λ 2 e λ 1 t λ 1 − λ 2 a=\frac {\lambda_1e^{\lambda_2t}-\lambda_2e^{\lambda_1t}} {\lambda_1-\lambda_2} a=λ1λ2λ1eλ2tλ2eλ1t b = e λ 1 t − e λ 2 t λ 1 − λ 2 b=\frac {e^{\lambda_1t}-e^{\lambda_2t}} {\lambda_1-\lambda_2} b=λ1λ2eλ1teλ2t
2、试证明:
P ˙ − 1 = − P − 1 P ˙ P − 1 {\dot{\boldsymbol P}}^{-1}=-{\boldsymbol P}^{-1}\dot{\boldsymbol P}{\boldsymbol P}^{-1} P˙1=P1P˙P1
② 若 R ( t ) {\boldsymbol R}(t) R(t) 为时变正交矩阵,且满足 R ˙ ( t ) R T ( t ) = S ( t ) \dot{\boldsymbol R}(t){\boldsymbol R}^{T}(t)={\boldsymbol S}(t) R˙(t)RT(t)=S(t),求证 S ( t ) {\boldsymbol S}(t) S(t) 为反对称阵。

参考答案如下:

Kalman滤波学习笔记二《数学基础:向量与矩阵》_第2张图片

你可能感兴趣的:(概率论与数理统计)