使用MATLAB对《最优估计基础》例题进行计算

        本人为导航工程专业学生，本学期学校开设了最优估计基础课程，使用的教材为《最优估计基础》，考虑到书中许多公式稍加改造并附上输入与输出接口后可应用于简单的实际问题，因此写下此文章用于记录本人在课程中使用matlab和书中的一些结论性公式与计算方法，如有错误还请各位指出。

目录

一、预备知识

二、最小二乘的计算

三、动态系统可控性与可测性的计算

四、卡尔曼滤波

---

一、预备知识

1.matlab创建矩阵的方法

使用[ ]创建矩阵，如：

A=[1 0;2 3];

则得到结果：

2.matlab创建对角阵方法

使用 diag()函数创建对角矩阵，如：

P=[1/3.5 1/2.7 1/4.0 1/3.0 1/2.5]
P=diag(P)

 则得到结果：

 3.matlab求逆阵的方法

使用inv()函数对矩阵取逆，如：

a2=[ 0.9061   -0.2500
   -0.2500    0.9833]

a3=inv(a2)

 则得到结果：

4.matlab求转置矩阵的方法

在矩阵前加" '  "，如：

B=[1 0;-1 0;-1 1;0 1;0 1]

a=B'

 则得到结果：

 5.matlab进行增广矩阵的操作

使用 “，”对矩阵进行横向增广，使用“；”对矩阵进行纵向增广，如：

a1=[1 2;3 4];
a2=[5 6;7 8];
a3=[a1,a2]
a4=[a1;a2]

 则得到结果：

 6.使用matlab创建合适的单位矩阵

使用eye()函数创建单位阵，使用size()函数获取矩阵的行列数，如：

H=[2 1;-1 2]
D=eye(size(H))

则得到结果：

7.使用matlab创建符号变量

使用syms函数创建符号变量，如：

syms s

则得到结果：

8.使用matlab进行拉普拉斯逆变换

使用函数ilaplace()函数对矩阵进行拉普拉斯逆变换，如：

syms s
D=eye(size(H));
ph2=ilaplace(inv(s*D-H))

 则得到结果：

 9.使用拉普拉斯逆变换法求状态转移矩阵的方法

使用状态方程的系数阵的单位矩阵与拉普拉斯变换中的s变量相乘并减去这个系数矩阵，对这个矩阵的逆阵求拉普拉斯逆变换，所得矩阵为状态转移矩阵，如：

已知系统的状态方程为𝑋^=[0 1;0 0]x,初始条件为x（0），试求状态转移矩阵

泰勒级数展开法为：

 

 拉普拉斯逆变换法：

syms s
A=[0 1;0 0];
D=eye(size(A));
ph2=ilaplace(inv(s*D-A))

 结果为：

 

二、最小二乘的计算

 1.最小二乘基本公式

最小二乘准则：残差平方和最小

 满足最小二乘准则的目标方程

 参数X的最小二乘估值为：

 2.例题

已知A点高程Ha=237.483m，B点高程247.120m，各段的观测值与各段之间的距离如表所示，每段的观测值具有不同的权重，用最小二乘法求C，与D的高程

 

 (1)根据已知信息列出残差方程

即：

编写程序：

clear;
B=[1 0;-1 0;-1 1;0 1;0 1];
P=[1/3.5 1/2.7 1/4.0 1/3.0 1/2.5];
P=diag(P);
L=[243.323;-243.330;-3.580;239.743;239.750];
xls=inv(B'*P*B)*B'*P*L

 运行结果如下

三、动态系统可控性与可测性的计算

 1.动态系统可控性的计算公式

（1）连续线性时不变系统的可控性条件

即：

 则系统可控

（2）离散线性时不变系统的可控性条件

 

 即：

则系统可控

例1：

 编写程序：

clear;
A=[1 0;2 3]
B=[0;2];
a1=(A^0)*B
a2=A*B;
b=A*A*B;
d=[a1,a2]
c=rank(d）

 结果为增广矩阵秩为1≠n因此该系统不可控

例2：

编写程序：

clear;
H2=[2 1];
A2=[1 -1;1 1];
M2=[H2;H2*A2];
n2=rank(M2)

 结果为增广矩阵秩为2=n因此该系统可控

  2.动态系统可测性的计算公式

 （1）连续线性时不变系统的可测性

 则系统可测

（2）离散线性时不变系统的可测性

例1：

 编写程序：

clear;
H1=[1 0];
A1=[-2 0;0 -3];
M1=[H1;H1*A1];
n1=rank(M1)

 结果为增广矩阵秩为1≠n因此该系统不可测

例2：

 编写程序：

H3=[0 1 0];
A3=[1 0 -1
    0 -2 1
    3 0 2];
M3=[H3;H3*A3;H3*A3*A3];
n3=rank(M3)

 结果为增广矩阵秩为3=n因此该系统可测

四、卡尔曼滤波

（1）卡尔曼滤波基本公式

 （2）典型例题

例：

已知离散线性系统的状态方程和观测方程如下：

 并已知：

 试求卡尔曼滤波估计和滤波误差方差阵

编写程序：

Xk0=[2.5;-4.5];
Dx0=[4 0;0 1];
z1=0.8;
Dk1=[1 0;0 0];
Ddeta=1;
H=[0 1];
f=[3 1;-2 1];
L=[1 0;0 0];
Xk1=f*Xk0;
Dk=f*Dx0*f'+L*Dk1*L'
K=Dk*H'*inv(H*Dk*H'+Ddeta);
V=z1-H*Xk1;
X=Xk1+K*V;
Dx=(eye(size(K*H))-K*H)*Dk

 结果为：