Shao Rui / Mbed 2 deprecated Kalman

1

Dependencies:   mbed

kalmanfilter/kalmanfilter.cpp@0:c55310328157, 2020-02-07 (annotated)

Committer:
shaorui
Date:
Fri Feb 07 11:34:24 2020 +0000
Revision:
0:c55310328157
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shaorui 0:c55310328157 1 #include"kalmanfilter.h"
shaorui 0:c55310328157 2 #include "math.h"
shaorui 0:c55310328157 3 /*========================= Mat3Init ====================================*/
shaorui 0:c55310328157 4 Mat3 kalmanfilter::Mat3Init(Mat3 C, double dNum){
shaorui 0:c55310328157 5 /*初始化3维方阵所有分量为指定值
shaorui 0:c55310328157 6 参数：[in]C: 待初始化的3维方阵
shaorui 0:c55310328157 7 [in]dNum: 指定值
shaorui 0:c55310328157 8 返回：C
shaorui 0:c55310328157 9 */
shaorui 0:c55310328157 10
shaorui 0:c55310328157 11 /*需求：
shaorui 0:c55310328157 12 类型: Mat3
shaorui 0:c55310328157 13 */
shaorui 0:c55310328157 14
shaorui 0:c55310328157 15 /*创建时间: 20070530
shaorui 0:c55310328157 16 最近更改时间：20070530
shaorui 0:c55310328157 17 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 18 Email: nostalgica@163.com
shaorui 0:c55310328157 19 */
shaorui 0:c55310328157 20
shaorui 0:c55310328157 21 int i,j;
shaorui 0:c55310328157 22 for (i=0;i<3;i++){
shaorui 0:c55310328157 23 for(j=0;i<3;j++){
shaorui 0:c55310328157 24 C[i][j]=dNum;
shaorui 0:c55310328157 25 }
shaorui 0:c55310328157 26 }
shaorui 0:c55310328157 27 return C;
shaorui 0:c55310328157 28 }
shaorui 0:c55310328157 29
shaorui 0:c55310328157 30 /*========================= MatInit ====================================*/
shaorui 0:c55310328157 31 Mat kalmanfilter:: MatInit(Mat C, int nM,int nN, double dNum){
shaorui 0:c55310328157 32 /*初始化矩阵所有分量为指定值
shaorui 0:c55310328157 33 参数：[in]C: 待初始化矩阵
shaorui 0:c55310328157 34 [in]nM: 矩阵行数
shaorui 0:c55310328157 35 [in]nN: 矩阵列数
shaorui 0:c55310328157 36 [in]dNum: 指定值
shaorui 0:c55310328157 37 返回：C
shaorui 0:c55310328157 38 */
shaorui 0:c55310328157 39
shaorui 0:c55310328157 40 /*需求：
shaorui 0:c55310328157 41 类型: Mat
shaorui 0:c55310328157 42 宏: MGet
shaorui 0:c55310328157 43 */
shaorui 0:c55310328157 44
shaorui 0:c55310328157 45 /*创建时间: 20070530
shaorui 0:c55310328157 46 最近更改时间：20091103
shaorui 0:c55310328157 47 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 48 Email: nostalgica@163.com
shaorui 0:c55310328157 49 */
shaorui 0:c55310328157 50
shaorui 0:c55310328157 51 int i,j;
shaorui 0:c55310328157 52 for (i=0;i<nM;i++){
shaorui 0:c55310328157 53 for(j=0;j<nN;j++){
shaorui 0:c55310328157 54 MGet(C,i,j,nN)=dNum;
shaorui 0:c55310328157 55 }
shaorui 0:c55310328157 56 }
shaorui 0:c55310328157 57 return C;
shaorui 0:c55310328157 58 }
shaorui 0:c55310328157 59
shaorui 0:c55310328157 60 /*========================= Mat3InitI ====================================*/
shaorui 0:c55310328157 61 Mat3 kalmanfilter::Mat3InitI(Mat3 C){
shaorui 0:c55310328157 62 /*初始化3维方阵为单位对角阵
shaorui 0:c55310328157 63 参数：[in]C: 待初始化的3维方阵
shaorui 0:c55310328157 64 返回：C
shaorui 0:c55310328157 65 */
shaorui 0:c55310328157 66
shaorui 0:c55310328157 67 /*需求：
shaorui 0:c55310328157 68 类型: Mat3
shaorui 0:c55310328157 69 */
shaorui 0:c55310328157 70
shaorui 0:c55310328157 71 /*创建时间: 20070530
shaorui 0:c55310328157 72 最近更改时间：20070530
shaorui 0:c55310328157 73 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 74 Email: nostalgica@163.com
shaorui 0:c55310328157 75 */
shaorui 0:c55310328157 76
shaorui 0:c55310328157 77 int i,j;
shaorui 0:c55310328157 78 for (i=0;i<3;i++){
shaorui 0:c55310328157 79 for(j=0;i<3;j++){
shaorui 0:c55310328157 80 if (i==j){
shaorui 0:c55310328157 81 C[i][j]=1.0;
shaorui 0:c55310328157 82 }else{
shaorui 0:c55310328157 83 C[i][j]=0.0;
shaorui 0:c55310328157 84 }
shaorui 0:c55310328157 85 }
shaorui 0:c55310328157 86 }
shaorui 0:c55310328157 87 return C;
shaorui 0:c55310328157 88 }
shaorui 0:c55310328157 89
shaorui 0:c55310328157 90
shaorui 0:c55310328157 91 /*========================= MatInitI ====================================*/
shaorui 0:c55310328157 92 Mat kalmanfilter::MatInitI(Mat C, int nOrder){
shaorui 0:c55310328157 93 /*方阵初始化为单位对角阵
shaorui 0:c55310328157 94 参数：[in]C: 待初始化的方阵
shaorui 0:c55310328157 95 [in]nOrder: 方阵维数
shaorui 0:c55310328157 96 返回：C
shaorui 0:c55310328157 97 */
shaorui 0:c55310328157 98
shaorui 0:c55310328157 99 /*需求：
shaorui 0:c55310328157 100 类型: Mat
shaorui 0:c55310328157 101 宏: MGet
shaorui 0:c55310328157 102 */
shaorui 0:c55310328157 103
shaorui 0:c55310328157 104 /*创建时间: 20070530
shaorui 0:c55310328157 105 最近更改时间：20070530
shaorui 0:c55310328157 106 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 107 Email: nostalgica@163.com
shaorui 0:c55310328157 108 */
shaorui 0:c55310328157 109
shaorui 0:c55310328157 110 int i,j;
shaorui 0:c55310328157 111 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 112 for(j=0;j<nOrder;j++){
shaorui 0:c55310328157 113 if (i==j){
shaorui 0:c55310328157 114 MGet(C,i,j,nOrder)=1.0;
shaorui 0:c55310328157 115 }else{
shaorui 0:c55310328157 116 MGet(C,i,j,nOrder)=0.0;
shaorui 0:c55310328157 117 }
shaorui 0:c55310328157 118 }
shaorui 0:c55310328157 119 }
shaorui 0:c55310328157 120 return C;
shaorui 0:c55310328157 121 }
shaorui 0:c55310328157 122
shaorui 0:c55310328157 123
shaorui 0:c55310328157 124 /*========================= Diag ====================================*/
shaorui 0:c55310328157 125 Mat kalmanfilter::Diag(cVec V, Mat Mout, int nOrder){
shaorui 0:c55310328157 126 /* 通过向量初始化对角方阵
shaorui 0:c55310328157 127 参数：[in]V: 向量V
shaorui 0:c55310328157 128 [out]Mout: 待初始化方阵
shaorui 0:c55310328157 129 [in]nOrder: 向量维数
shaorui 0:c55310328157 130 返回：Mout
shaorui 0:c55310328157 131 */
shaorui 0:c55310328157 132
shaorui 0:c55310328157 133 /*需求：
shaorui 0:c55310328157 134 类型: Mat, cVec
shaorui 0:c55310328157 135 宏: MGet
shaorui 0:c55310328157 136 */
shaorui 0:c55310328157 137
shaorui 0:c55310328157 138 /*创建时间: 20080509
shaorui 0:c55310328157 139 最近更改时间：20080509
shaorui 0:c55310328157 140 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 141 Email: nostalgica@163.com
shaorui 0:c55310328157 142 */
shaorui 0:c55310328157 143
shaorui 0:c55310328157 144 int i,j;
shaorui 0:c55310328157 145 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 146 for(j=0;j<nOrder;j++){
shaorui 0:c55310328157 147 if (i==j){
shaorui 0:c55310328157 148 MGet(Mout,i,j,nOrder)=V[i];
shaorui 0:c55310328157 149 }else{
shaorui 0:c55310328157 150 MGet(Mout,i,j,nOrder)=0.0;
shaorui 0:c55310328157 151 }
shaorui 0:c55310328157 152 }
shaorui 0:c55310328157 153 }
shaorui 0:c55310328157 154 return Mout;
shaorui 0:c55310328157 155 }
shaorui 0:c55310328157 156
shaorui 0:c55310328157 157 /******************************************************************************************************************/
shaorui 0:c55310328157 158
shaorui 0:c55310328157 159 /*============================ Mat3Sum=================================*/
shaorui 0:c55310328157 160 Mat3 kalmanfilter::Mat3Sum(cMat3 C1, cMat3 C2, Mat3 C){
shaorui 0:c55310328157 161 /*3*3矩阵加法
shaorui 0:c55310328157 162 参数：[in]C1:3*3 C1矩阵
shaorui 0:c55310328157 163 [in]C2:3*3 C2矩阵
shaorui 0:c55310328157 164 [out]C: C1+C2
shaorui 0:c55310328157 165 返回：C
shaorui 0:c55310328157 166 */
shaorui 0:c55310328157 167
shaorui 0:c55310328157 168 /*需求：
shaorui 0:c55310328157 169 类型: Mat3,cMat3
shaorui 0:c55310328157 170 */
shaorui 0:c55310328157 171
shaorui 0:c55310328157 172 /* 创建时间: 20070528
shaorui 0:c55310328157 173 最近更改时间：20070528
shaorui 0:c55310328157 174 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 175 */
shaorui 0:c55310328157 176 int i,j;
shaorui 0:c55310328157 177 for (i=0;i<3;i++)
shaorui 0:c55310328157 178 {
shaorui 0:c55310328157 179 for(j=0;j<3;j++) C[i][j] = C1[i][j] + C2[i][j];
shaorui 0:c55310328157 180 }
shaorui 0:c55310328157 181 return C;
shaorui 0:c55310328157 182 }
shaorui 0:c55310328157 183
shaorui 0:c55310328157 184
shaorui 0:c55310328157 185 /*============================ Mat3Sum3=================================*/
shaorui 0:c55310328157 186 Mat3 kalmanfilter::Mat3Sum3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C){
shaorui 0:c55310328157 187 /*3*3矩阵加法(三个矩阵)
shaorui 0:c55310328157 188 参数：[in]C1:3*3 C1矩阵
shaorui 0:c55310328157 189 [in]C2:3*3 C2矩阵
shaorui 0:c55310328157 190 [in]C3:3*3 C3矩阵
shaorui 0:c55310328157 191 [out]C: C1+C2+C3
shaorui 0:c55310328157 192 返回：C
shaorui 0:c55310328157 193 */
shaorui 0:c55310328157 194
shaorui 0:c55310328157 195 /*需求：
shaorui 0:c55310328157 196 类型: Mat3,cMat3
shaorui 0:c55310328157 197 */
shaorui 0:c55310328157 198
shaorui 0:c55310328157 199 /* 创建时间: 20080410
shaorui 0:c55310328157 200 最近更改时间：20080410
shaorui 0:c55310328157 201 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 202 */
shaorui 0:c55310328157 203 int i,j;
shaorui 0:c55310328157 204 for (i=0;i<3;i++)
shaorui 0:c55310328157 205 {
shaorui 0:c55310328157 206 for(j=0;j<3;j++) C[i][j] = C1[i][j] + C2[i][j]+ C3[i][j];
shaorui 0:c55310328157 207 }
shaorui 0:c55310328157 208 return C;
shaorui 0:c55310328157 209 }
shaorui 0:c55310328157 210
shaorui 0:c55310328157 211
shaorui 0:c55310328157 212 /*============================ MatSum=================================*/
shaorui 0:c55310328157 213 Mat kalmanfilter::MatSum(cMat C1, cMat C2, Mat C, int nM, int nN){
shaorui 0:c55310328157 214 /*矩阵加法
shaorui 0:c55310328157 215 参数：[in]C1: C1矩阵
shaorui 0:c55310328157 216 [in]C2: C2矩阵
shaorui 0:c55310328157 217 [out]C: C1+C2
shaorui 0:c55310328157 218 [in]nM: 矩阵行数
shaorui 0:c55310328157 219 [in]nN: 矩阵列数
shaorui 0:c55310328157 220 返回：C
shaorui 0:c55310328157 221 */
shaorui 0:c55310328157 222
shaorui 0:c55310328157 223 /*需求：
shaorui 0:c55310328157 224 类型: Mat,cMat
shaorui 0:c55310328157 225 宏: MGet
shaorui 0:c55310328157 226 */
shaorui 0:c55310328157 227
shaorui 0:c55310328157 228 /* 创建时间: 20080402
shaorui 0:c55310328157 229 最近更改时间：20080402
shaorui 0:c55310328157 230 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 231 */
shaorui 0:c55310328157 232
shaorui 0:c55310328157 233 int i,j;
shaorui 0:c55310328157 234 for (i=0;i<nM;i++){
shaorui 0:c55310328157 235 for(j=0;j<nN;j++){
shaorui 0:c55310328157 236 MGet(C,i,j,nN)=MGet(C1,i,j,nN)+MGet(C2,i,j,nN);
shaorui 0:c55310328157 237 }
shaorui 0:c55310328157 238 }
shaorui 0:c55310328157 239 return C;
shaorui 0:c55310328157 240 }
shaorui 0:c55310328157 241
shaorui 0:c55310328157 242
shaorui 0:c55310328157 243 /*============================ MatSum3=================================*/
shaorui 0:c55310328157 244 Mat kalmanfilter::MatSum3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN){
shaorui 0:c55310328157 245 /*矩阵加法(三个矩阵)
shaorui 0:c55310328157 246 参数：[in]C1: C1矩阵
shaorui 0:c55310328157 247 [in]C2: C2矩阵
shaorui 0:c55310328157 248 [in]C3: C3矩阵
shaorui 0:c55310328157 249 [out]C: C1+C2+C3
shaorui 0:c55310328157 250 [in]nM: 矩阵行数
shaorui 0:c55310328157 251 [in]nN: 矩阵列数
shaorui 0:c55310328157 252 返回：C
shaorui 0:c55310328157 253 */
shaorui 0:c55310328157 254
shaorui 0:c55310328157 255 /*需求：
shaorui 0:c55310328157 256 类型: Mat,cMat
shaorui 0:c55310328157 257 宏: MGet
shaorui 0:c55310328157 258 */
shaorui 0:c55310328157 259
shaorui 0:c55310328157 260 /* 创建时间: 20080402
shaorui 0:c55310328157 261 最近更改时间：20080402
shaorui 0:c55310328157 262 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 263 */
shaorui 0:c55310328157 264
shaorui 0:c55310328157 265 int i,j;
shaorui 0:c55310328157 266 for (i=0;i<nM;i++){
shaorui 0:c55310328157 267 for(j=0;j<nN;j++){
shaorui 0:c55310328157 268 MGet(C,i,j,nN)=MGet(C1,i,j,nN)+MGet(C2,i,j,nN)+MGet(C3,i,j,nN);
shaorui 0:c55310328157 269 }
shaorui 0:c55310328157 270 }
shaorui 0:c55310328157 271 return C;
shaorui 0:c55310328157 272 }
shaorui 0:c55310328157 273
shaorui 0:c55310328157 274
shaorui 0:c55310328157 275 /*************************************************************************************************/
shaorui 0:c55310328157 276
shaorui 0:c55310328157 277 /*============================ Mat3Minus =================================*/
shaorui 0:c55310328157 278 Mat3 kalmanfilter::Mat3Minus(cMat3 C1, cMat3 C2, Mat3 C){
shaorui 0:c55310328157 279 /*3*3矩阵减法
shaorui 0:c55310328157 280 参数：[in]C1:3*3 C1矩阵
shaorui 0:c55310328157 281 [in]C2:3*3 C2矩阵
shaorui 0:c55310328157 282 [out]C: C1-C2
shaorui 0:c55310328157 283 返回：C
shaorui 0:c55310328157 284 */
shaorui 0:c55310328157 285
shaorui 0:c55310328157 286 /*需求：
shaorui 0:c55310328157 287 类型: Mat3,cMat3
shaorui 0:c55310328157 288 */
shaorui 0:c55310328157 289
shaorui 0:c55310328157 290 /* 创建时间: 20070528
shaorui 0:c55310328157 291 最近更改时间：20070528
shaorui 0:c55310328157 292 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 293 */
shaorui 0:c55310328157 294 int i,j;
shaorui 0:c55310328157 295 for (i=0;i<3;i++){
shaorui 0:c55310328157 296 for(j=0;j<3;j++) C[i][j] = C1[i][j] - C2[i][j];
shaorui 0:c55310328157 297 }
shaorui 0:c55310328157 298 return C;
shaorui 0:c55310328157 299 }
shaorui 0:c55310328157 300
shaorui 0:c55310328157 301
shaorui 0:c55310328157 302 /*============================ MatMinus =================================*/
shaorui 0:c55310328157 303 Mat kalmanfilter::MatMinus(cMat C1, cMat C2, Mat C, int nM, int nN){
shaorui 0:c55310328157 304 /*矩阵减法
shaorui 0:c55310328157 305 参数：[in]C1: C1矩阵
shaorui 0:c55310328157 306 [in]C2: C2矩阵
shaorui 0:c55310328157 307 [out]C: C1-C2
shaorui 0:c55310328157 308 [in]nM: 矩阵行数
shaorui 0:c55310328157 309 [in]nN: 矩阵列数
shaorui 0:c55310328157 310 返回：C
shaorui 0:c55310328157 311 */
shaorui 0:c55310328157 312
shaorui 0:c55310328157 313 /*需求：
shaorui 0:c55310328157 314 类型: Mat,cMat
shaorui 0:c55310328157 315 */
shaorui 0:c55310328157 316
shaorui 0:c55310328157 317 /* 创建时间: 20080402
shaorui 0:c55310328157 318 最近更改时间：20080402
shaorui 0:c55310328157 319 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 320 */
shaorui 0:c55310328157 321 int i;
shaorui 0:c55310328157 322 for (i=0;i<nM*nN;i++){
shaorui 0:c55310328157 323 C[i]=C1[i]-C2[i];
shaorui 0:c55310328157 324 }
shaorui 0:c55310328157 325
shaorui 0:c55310328157 326 return C;
shaorui 0:c55310328157 327 }
shaorui 0:c55310328157 328 /*********************************************************************************************************/
shaorui 0:c55310328157 329
shaorui 0:c55310328157 330 /*========================= Mat3Cpy ====================================*/
shaorui 0:c55310328157 331 Mat3 kalmanfilter::Mat3Cpy(Mat3 dest,cMat3 source){
shaorui 0:c55310328157 332 /*3*3矩阵拷贝
shaorui 0:c55310328157 333 参数：[out]dest:3*3目标矩阵
shaorui 0:c55310328157 334 [in]source:3*3源矩阵
shaorui 0:c55310328157 335 返回：dest
shaorui 0:c55310328157 336 */
shaorui 0:c55310328157 337
shaorui 0:c55310328157 338 /*需求：
shaorui 0:c55310328157 339 类型: Mat3,cMat3
shaorui 0:c55310328157 340 */
shaorui 0:c55310328157 341
shaorui 0:c55310328157 342 /*创建时间: 20070527
shaorui 0:c55310328157 343 最近更改时间：20070527
shaorui 0:c55310328157 344 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 345 Email: nostalgica@163.com
shaorui 0:c55310328157 346 */
shaorui 0:c55310328157 347
shaorui 0:c55310328157 348 dest[0][0]=source[0][0];
shaorui 0:c55310328157 349 dest[0][1]=source[0][1];
shaorui 0:c55310328157 350 dest[0][2]=source[0][2];
shaorui 0:c55310328157 351 dest[1][0]=source[1][0];
shaorui 0:c55310328157 352 dest[1][1]=source[1][1];
shaorui 0:c55310328157 353 dest[1][2]=source[1][2];
shaorui 0:c55310328157 354 dest[2][0]=source[2][0];
shaorui 0:c55310328157 355 dest[2][1]=source[2][1];
shaorui 0:c55310328157 356 dest[2][2]=source[2][2];
shaorui 0:c55310328157 357 return dest;
shaorui 0:c55310328157 358 }
shaorui 0:c55310328157 359
shaorui 0:c55310328157 360
shaorui 0:c55310328157 361 /*========================= MatCpy ====================================*/
shaorui 0:c55310328157 362 Mat kalmanfilter::MatCpy(Mat dest, cMat source, int nM, int nN){
shaorui 0:c55310328157 363 /*矩阵拷贝
shaorui 0:c55310328157 364 参数：[out]dest: 目标矩阵
shaorui 0:c55310328157 365 [in]source: 源矩阵
shaorui 0:c55310328157 366 [in]nM: 矩阵行数
shaorui 0:c55310328157 367 [in]nN: 矩阵列数
shaorui 0:c55310328157 368 返回：dest
shaorui 0:c55310328157 369 */
shaorui 0:c55310328157 370
shaorui 0:c55310328157 371 /*需求：
shaorui 0:c55310328157 372 类型: Mat,cMat
shaorui 0:c55310328157 373 */
shaorui 0:c55310328157 374
shaorui 0:c55310328157 375 /*创建时间: 20080402
shaorui 0:c55310328157 376 最近更改时间：20080402
shaorui 0:c55310328157 377 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 378 Email: nostalgica@163.com
shaorui 0:c55310328157 379 */
shaorui 0:c55310328157 380
shaorui 0:c55310328157 381 int i;
shaorui 0:c55310328157 382 for(i=0;i<nM*nN;i++){
shaorui 0:c55310328157 383 dest[i]=source[i];
shaorui 0:c55310328157 384 }
shaorui 0:c55310328157 385 return dest;
shaorui 0:c55310328157 386 }
shaorui 0:c55310328157 387 /***************************************************************************************************/
shaorui 0:c55310328157 388
shaorui 0:c55310328157 389 /*============================ Mat3Prod =================================*/
shaorui 0:c55310328157 390 Mat3 kalmanfilter::Mat3Prod(cMat3 C1, cMat3 C2, Mat3 C){
shaorui 0:c55310328157 391 /*3*3矩阵乘法
shaorui 0:c55310328157 392 参数：[in]C1:3*3 C1矩阵
shaorui 0:c55310328157 393 [in]C2:3*3 C2矩阵
shaorui 0:c55310328157 394 [out]C: C1*C2
shaorui 0:c55310328157 395 返回：C
shaorui 0:c55310328157 396 */
shaorui 0:c55310328157 397
shaorui 0:c55310328157 398 /*需求：
shaorui 0:c55310328157 399 类型：Mat3,cMat3
shaorui 0:c55310328157 400 */
shaorui 0:c55310328157 401
shaorui 0:c55310328157 402 /* 创建时间: 20070528
shaorui 0:c55310328157 403 最近更改时间：20070528
shaorui 0:c55310328157 404 Copyright 2005-2010 HLW
shaorui 0:c55310328157 405 */
shaorui 0:c55310328157 406
shaorui 0:c55310328157 407 int i,j,k;
shaorui 0:c55310328157 408 double Ctemp[3][3];
shaorui 0:c55310328157 409 for (i=0;i<3;i++){
shaorui 0:c55310328157 410 for(j=0;j<3;j++){
shaorui 0:c55310328157 411 Ctemp[i][j] = 0.0;
shaorui 0:c55310328157 412 for (k=0;k<3;k++){
shaorui 0:c55310328157 413 Ctemp[i][j] = Ctemp[i][j] + C1[i][k]*C2[k][j];
shaorui 0:c55310328157 414 }
shaorui 0:c55310328157 415 }
shaorui 0:c55310328157 416 }
shaorui 0:c55310328157 417 for (i=0;i<3;i++){
shaorui 0:c55310328157 418 for(j=0;j<3;j++){
shaorui 0:c55310328157 419 C[i][j]=Ctemp[i][j];
shaorui 0:c55310328157 420 }
shaorui 0:c55310328157 421 }
shaorui 0:c55310328157 422 return C;
shaorui 0:c55310328157 423 }
shaorui 0:c55310328157 424
shaorui 0:c55310328157 425
shaorui 0:c55310328157 426 /*============================ Mat3Prod3 =================================*/
shaorui 0:c55310328157 427 Mat3 kalmanfilter::Mat3Prod3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C){
shaorui 0:c55310328157 428 /*3*3矩阵乘法(三个矩阵)
shaorui 0:c55310328157 429 参数：[in]C1:3*3 C1矩阵
shaorui 0:c55310328157 430 [in]C2:3*3 C2矩阵
shaorui 0:c55310328157 431 [in]C3:3*3 C3矩阵
shaorui 0:c55310328157 432 [out]C: C1*C2*C3
shaorui 0:c55310328157 433 返回：C
shaorui 0:c55310328157 434 */
shaorui 0:c55310328157 435
shaorui 0:c55310328157 436 /*需求：
shaorui 0:c55310328157 437 类型：Mat3,cMat3
shaorui 0:c55310328157 438 */
shaorui 0:c55310328157 439
shaorui 0:c55310328157 440 /* 创建时间: 20080410
shaorui 0:c55310328157 441 最近更改时间: 20080410
shaorui 0:c55310328157 442 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 443 */
shaorui 0:c55310328157 444
shaorui 0:c55310328157 445 int i,j,k;
shaorui 0:c55310328157 446 double Ctemp[3][3],Ctemp2[3][3];
shaorui 0:c55310328157 447 for (i=0;i<3;i++){
shaorui 0:c55310328157 448 for(j=0;j<3;j++){
shaorui 0:c55310328157 449 Ctemp[i][j] = 0.0;
shaorui 0:c55310328157 450 for (k=0;k<3;k++){
shaorui 0:c55310328157 451 Ctemp[i][j] = Ctemp[i][j] + C1[i][k]*C2[k][j];
shaorui 0:c55310328157 452 }
shaorui 0:c55310328157 453 }
shaorui 0:c55310328157 454 }
shaorui 0:c55310328157 455 for (i=0;i<3;i++){
shaorui 0:c55310328157 456 for(j=0;j<3;j++){
shaorui 0:c55310328157 457 Ctemp2[i][j] = 0.0;
shaorui 0:c55310328157 458 for (k=0;k<3;k++){
shaorui 0:c55310328157 459 Ctemp2[i][j] = Ctemp2[i][j] + Ctemp[i][k]*C3[k][j];
shaorui 0:c55310328157 460 }
shaorui 0:c55310328157 461 }
shaorui 0:c55310328157 462 }
shaorui 0:c55310328157 463 for (i=0;i<3;i++){
shaorui 0:c55310328157 464 for(j=0;j<3;j++){
shaorui 0:c55310328157 465 C[i][j]=Ctemp2[i][j];
shaorui 0:c55310328157 466 }
shaorui 0:c55310328157 467 }
shaorui 0:c55310328157 468 return C;
shaorui 0:c55310328157 469 }
shaorui 0:c55310328157 470
shaorui 0:c55310328157 471
shaorui 0:c55310328157 472 /*============================ MatProd =================================*/
shaorui 0:c55310328157 473 Mat kalmanfilter::MatProd(cMat C1, cMat C2, Mat C, int nM, int nN, int nP){
shaorui 0:c55310328157 474 /*矩阵乘法
shaorui 0:c55310328157 475 参数：[in]C1: C1矩阵(nM*nN)
shaorui 0:c55310328157 476 [in]C2: C2矩阵(nN*nP)
shaorui 0:c55310328157 477 [out]C: C1*C2
shaorui 0:c55310328157 478 [in]nM: C1行数
shaorui 0:c55310328157 479 [in]nN: C1列数(C2行数)
shaorui 0:c55310328157 480 [in]nP: C2列数
shaorui 0:c55310328157 481 返回：C
shaorui 0:c55310328157 482 */
shaorui 0:c55310328157 483
shaorui 0:c55310328157 484 /*需求：
shaorui 0:c55310328157 485 类型: Mat,cMat
shaorui 0:c55310328157 486 头文件: stdlib.h
shaorui 0:c55310328157 487 宏: MGet
shaorui 0:c55310328157 488 */
shaorui 0:c55310328157 489
shaorui 0:c55310328157 490 /* 创建时间: 20080403
shaorui 0:c55310328157 491 最近更改时间：20080403
shaorui 0:c55310328157 492 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 493 */
shaorui 0:c55310328157 494
shaorui 0:c55310328157 495 int i,j,k;
shaorui 0:c55310328157 496 Mat Ctemp=(Mat)malloc(sizeof(double)*nM*nP);
shaorui 0:c55310328157 497 for (i=0;i<nM;i++){
shaorui 0:c55310328157 498 for(j=0;j<nP;j++){
shaorui 0:c55310328157 499 MGet(Ctemp,i,j,nP) = 0.0;
shaorui 0:c55310328157 500 for (k=0;k<nN;k++){
shaorui 0:c55310328157 501 MGet(Ctemp,i,j,nP) = MGet(Ctemp,i,j,nP) + MGet(C1,i,k,nN)*MGet(C2,k,j,nP);
shaorui 0:c55310328157 502 }
shaorui 0:c55310328157 503 }
shaorui 0:c55310328157 504 }
shaorui 0:c55310328157 505 for (i=0;i<nM;i++){
shaorui 0:c55310328157 506 for(j=0;j<nP;j++){
shaorui 0:c55310328157 507 MGet(C,i,j,nP)=MGet(Ctemp,i,j,nP);
shaorui 0:c55310328157 508 }
shaorui 0:c55310328157 509 }
shaorui 0:c55310328157 510 free(Ctemp);
shaorui 0:c55310328157 511 return C;
shaorui 0:c55310328157 512 }
shaorui 0:c55310328157 513
shaorui 0:c55310328157 514
shaorui 0:c55310328157 515 /*============================ MatProd3 =================================*/
shaorui 0:c55310328157 516 Mat kalmanfilter::MatProd3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN, int nP, int nQ){
shaorui 0:c55310328157 517 /*矩阵乘法(三个矩阵)
shaorui 0:c55310328157 518 参数：[in]C1: C1矩阵(nM*nN)
shaorui 0:c55310328157 519 [in]C2: C2矩阵(nN*nP)
shaorui 0:c55310328157 520 [in]C3: C3矩阵(nP*nQ)
shaorui 0:c55310328157 521 [out]C: C1*C2*C3
shaorui 0:c55310328157 522 [in]nM: C1行数
shaorui 0:c55310328157 523 [in]nN: C1列数(C2行数)
shaorui 0:c55310328157 524 [in]nP: C2列数(C3行数)
shaorui 0:c55310328157 525 [in]nQ: C3列数
shaorui 0:c55310328157 526 返回：C
shaorui 0:c55310328157 527 */
shaorui 0:c55310328157 528
shaorui 0:c55310328157 529 /*需求：
shaorui 0:c55310328157 530 类型: Mat,cMat
shaorui 0:c55310328157 531 头文件: stdlib.h
shaorui 0:c55310328157 532 宏: MGet
shaorui 0:c55310328157 533 */
shaorui 0:c55310328157 534
shaorui 0:c55310328157 535 /* 创建时间: 20080410
shaorui 0:c55310328157 536 最近更改时间：20080410
shaorui 0:c55310328157 537 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 538 */
shaorui 0:c55310328157 539
shaorui 0:c55310328157 540 int i,j,k;
shaorui 0:c55310328157 541 Mat Ctemp=(Mat)malloc(sizeof(double)*nM*nP);
shaorui 0:c55310328157 542 Mat Ctemp2=(Mat)malloc(sizeof(double)*nM*nQ);
shaorui 0:c55310328157 543 for (i=0;i<nM;i++){
shaorui 0:c55310328157 544 for(j=0;j<nP;j++){
shaorui 0:c55310328157 545 MGet(Ctemp,i,j,nP) = 0.0;
shaorui 0:c55310328157 546 for (k=0;k<nN;k++){
shaorui 0:c55310328157 547 MGet(Ctemp,i,j,nP) = MGet(Ctemp,i,j,nP) + MGet(C1,i,k,nN)*MGet(C2,k,j,nP);
shaorui 0:c55310328157 548 }
shaorui 0:c55310328157 549 }
shaorui 0:c55310328157 550 }
shaorui 0:c55310328157 551 for (i=0;i<nM;i++){
shaorui 0:c55310328157 552 for(j=0;j<nQ;j++){
shaorui 0:c55310328157 553 MGet(Ctemp2,i,j,nQ) = 0.0;
shaorui 0:c55310328157 554 for (k=0;k<nP;k++){
shaorui 0:c55310328157 555 MGet(Ctemp2,i,j,nQ) = MGet(Ctemp2,i,j,nQ) + MGet(Ctemp,i,k,nP)*MGet(C3,k,j,nQ);
shaorui 0:c55310328157 556 }
shaorui 0:c55310328157 557 }
shaorui 0:c55310328157 558 }
shaorui 0:c55310328157 559 for (i=0;i<nM;i++){
shaorui 0:c55310328157 560 for(j=0;j<nQ;j++){
shaorui 0:c55310328157 561 MGet(C,i,j,nQ)=MGet(Ctemp2,i,j,nQ);
shaorui 0:c55310328157 562 }
shaorui 0:c55310328157 563 }
shaorui 0:c55310328157 564 free(Ctemp);
shaorui 0:c55310328157 565 free(Ctemp2);
shaorui 0:c55310328157 566 return C;
shaorui 0:c55310328157 567 }
shaorui 0:c55310328157 568 /***************************************************************************************************/
shaorui 0:c55310328157 569
shaorui 0:c55310328157 570 /*============================ Mat3Trans =================================*/
shaorui 0:c55310328157 571 Mat3 kalmanfilter::Mat3Trans(cMat3 C,Mat3 CT){
shaorui 0:c55310328157 572 /*3*3矩阵转置
shaorui 0:c55310328157 573 参数：[in]C: 3*3 C矩阵
shaorui 0:c55310328157 574 [out]CT: C的转置
shaorui 0:c55310328157 575 返回：CT
shaorui 0:c55310328157 576 */
shaorui 0:c55310328157 577
shaorui 0:c55310328157 578 /*需求：
shaorui 0:c55310328157 579 类型：Mat3,cMat3
shaorui 0:c55310328157 580 */
shaorui 0:c55310328157 581
shaorui 0:c55310328157 582 /* 创建时间: 20070528
shaorui 0:c55310328157 583 最近更改时间：20070528
shaorui 0:c55310328157 584 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 585 */
shaorui 0:c55310328157 586
shaorui 0:c55310328157 587 int i,j;
shaorui 0:c55310328157 588 double Ctemp[3][3];
shaorui 0:c55310328157 589 for(i=0;i<3;i++){
shaorui 0:c55310328157 590 for(j=0;j<3;j++){
shaorui 0:c55310328157 591 Ctemp[i][j] = C[j][i];
shaorui 0:c55310328157 592 }
shaorui 0:c55310328157 593 }
shaorui 0:c55310328157 594 for(i=0;i<3;i++){
shaorui 0:c55310328157 595 for(j=0;j<3;j++){
shaorui 0:c55310328157 596 CT[i][j] = Ctemp[i][j];
shaorui 0:c55310328157 597 }
shaorui 0:c55310328157 598 }
shaorui 0:c55310328157 599 return CT;
shaorui 0:c55310328157 600 }
shaorui 0:c55310328157 601
shaorui 0:c55310328157 602
shaorui 0:c55310328157 603 /*============================ MatTrans =================================*/
shaorui 0:c55310328157 604 Mat kalmanfilter::MatTrans(cMat C,Mat CT,int nM,int nN){
shaorui 0:c55310328157 605 /*矩阵转置
shaorui 0:c55310328157 606 参数：[in]C: C矩阵
shaorui 0:c55310328157 607 [out]CT: C的转置
shaorui 0:c55310328157 608 [in]nM: C矩阵行数
shaorui 0:c55310328157 609 [in]nN: C矩阵列数
shaorui 0:c55310328157 610 返回：CT
shaorui 0:c55310328157 611 */
shaorui 0:c55310328157 612
shaorui 0:c55310328157 613 /*需求：
shaorui 0:c55310328157 614 类型：Mat,cMat
shaorui 0:c55310328157 615 头文件：stdlib.h
shaorui 0:c55310328157 616 宏：MGet
shaorui 0:c55310328157 617 */
shaorui 0:c55310328157 618
shaorui 0:c55310328157 619 /* 创建时间: 20080402
shaorui 0:c55310328157 620 最近更改时间：20080402
shaorui 0:c55310328157 621 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 622 */
shaorui 0:c55310328157 623
shaorui 0:c55310328157 624 int i,j;
shaorui 0:c55310328157 625 Mat Ctemp=(Mat)malloc(sizeof(double)*nM*nN);
shaorui 0:c55310328157 626 for(i=0;i<nN;i++){
shaorui 0:c55310328157 627 for(j=0;j<nM;j++){
shaorui 0:c55310328157 628 MGet(Ctemp,i,j,nM)=MGet(C,j,i,nN);
shaorui 0:c55310328157 629 }
shaorui 0:c55310328157 630 }
shaorui 0:c55310328157 631 for(i=0;i<nM*nN;i++){
shaorui 0:c55310328157 632 CT[i] = Ctemp[i];
shaorui 0:c55310328157 633 }
shaorui 0:c55310328157 634 free(Ctemp);
shaorui 0:c55310328157 635 return CT;
shaorui 0:c55310328157 636 }
shaorui 0:c55310328157 637 /**************************************************************************************************/
shaorui 0:c55310328157 638
shaorui 0:c55310328157 639 /*============================ MatInv =================================*/
shaorui 0:c55310328157 640 Mat kalmanfilter::MatInv(cMat C,Mat IC,int n){
shaorui 0:c55310328157 641 /*方阵的逆
shaorui 0:c55310328157 642 参数：[in]C:方阵C
shaorui 0:c55310328157 643 [out]IC:方阵C的逆
shaorui 0:c55310328157 644 [in]n:方阵C的维数
shaorui 0:c55310328157 645 返回：IC,若不可逆或失败则返回NULL
shaorui 0:c55310328157 646 */
shaorui 0:c55310328157 647
shaorui 0:c55310328157 648 /*需求：
shaorui 0:c55310328157 649 头文件:math.h,stdlib.h
shaorui 0:c55310328157 650 类型:Mat,cMat
shaorui 0:c55310328157 651 宏:NULL
shaorui 0:c55310328157 652 */
shaorui 0:c55310328157 653
shaorui 0:c55310328157 654 /* 创建时间: 20070615
shaorui 0:c55310328157 655 最近更改时间: 20070615
shaorui 0:c55310328157 656 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 657 */
shaorui 0:c55310328157 658
shaorui 0:c55310328157 659 int i,j,k,l;
shaorui 0:c55310328157 660 double *cpyC;
shaorui 0:c55310328157 661
shaorui 0:c55310328157 662 cpyC=(double*)malloc(n*n*sizeof(double));
shaorui 0:c55310328157 663 if (cpyC==NULL){
shaorui 0:c55310328157 664 return NULL;
shaorui 0:c55310328157 665 }
shaorui 0:c55310328157 666
shaorui 0:c55310328157 667 for (i=0;i<n*n;i++){
shaorui 0:c55310328157 668 cpyC[i]=C[i];
shaorui 0:c55310328157 669 }
shaorui 0:c55310328157 670 /* 单位阵*/
shaorui 0:c55310328157 671 for (i=0; i<n; i++){
shaorui 0:c55310328157 672 for (j=0; j<n; j++){
shaorui 0:c55310328157 673 *(IC+i*n+j) = 0.0;
shaorui 0:c55310328157 674 }
shaorui 0:c55310328157 675 *(IC+i*n+i) = 1.0;
shaorui 0:c55310328157 676 }
shaorui 0:c55310328157 677
shaorui 0:c55310328157 678 /* 化上三角阵*/
shaorui 0:c55310328157 679 for (j=0; j<n; j++) {
shaorui 0:c55310328157 680 if(fabs(*(cpyC+j*n+j))>1.0e-25){/* C[j][j]不等于0*/
shaorui 0:c55310328157 681 /* IC阵的第j行除以C[j][j]*/
shaorui 0:c55310328157 682 for(k=0; k<n; k++){
shaorui 0:c55310328157 683 *(IC+j*n+k) /= *(cpyC+j*n+j);
shaorui 0:c55310328157 684 }
shaorui 0:c55310328157 685 /* C阵的第j行除以C[j][j]*/
shaorui 0:c55310328157 686 for(k=n-1; k>=j; k--){
shaorui 0:c55310328157 687 *(cpyC+j*n+k) /= *(cpyC+j*n+j);
shaorui 0:c55310328157 688 }
shaorui 0:c55310328157 689
shaorui 0:c55310328157 690 for(i=j+1; i<n; i++){
shaorui 0:c55310328157 691 /* IC阵的第i行 - C[i][j]*IC阵的第j行*/
shaorui 0:c55310328157 692 for(k=0; k<n; k++){
shaorui 0:c55310328157 693 *(IC+i*n+k) -= *(cpyC+i*n+j) * *(IC+j*n+k);
shaorui 0:c55310328157 694 }
shaorui 0:c55310328157 695 /* C阵的第i行 - C[i][j]*C阵的第j行*/
shaorui 0:c55310328157 696 for (k=n-1; k>=j; k--){
shaorui 0:c55310328157 697 *(cpyC+i*n+k) -= *(cpyC+i*n+j) * *(cpyC+j*n+k);
shaorui 0:c55310328157 698 }
shaorui 0:c55310328157 699 }
shaorui 0:c55310328157 700 }else if (j<n-1){
shaorui 0:c55310328157 701 for(l=j+1; l<n; l++){
shaorui 0:c55310328157 702 /* 若C阵第j行后的C[l][j]不等于0，第j行加上第l行*/
shaorui 0:c55310328157 703 if (fabs(*(cpyC+l*n+j)) > 1.0e-25) {
shaorui 0:c55310328157 704 for (k=0; k<n; k++){
shaorui 0:c55310328157 705 *(IC+j*n+k) += *(IC+l*n+k);
shaorui 0:c55310328157 706 }
shaorui 0:c55310328157 707 for (k=n-1; k>=j; k--){
shaorui 0:c55310328157 708 *(cpyC+j*n+k) += *(cpyC+l*n+k);
shaorui 0:c55310328157 709 }
shaorui 0:c55310328157 710 /* IC阵的第j行除以C[j][j]*/
shaorui 0:c55310328157 711 for (k=0; k<n; k++){
shaorui 0:c55310328157 712 *(IC+j*n+k) /= *(cpyC+j*n+j);
shaorui 0:c55310328157 713 }
shaorui 0:c55310328157 714 /* C阵的第j行除以C[j][j]*/
shaorui 0:c55310328157 715 for (k=n-1; k>=j; k--){
shaorui 0:c55310328157 716 *(cpyC+j*n+k) /= *(cpyC+j*n+j);
shaorui 0:c55310328157 717 }
shaorui 0:c55310328157 718 /* 第i行 - C[i][j]*第j行*/
shaorui 0:c55310328157 719 for (i=j+1; i<n; i++){
shaorui 0:c55310328157 720 for (k=0; k<n; k++){
shaorui 0:c55310328157 721 *(IC+i*n+k) -= *(cpyC+i*n+j) * *(IC+j*n+k);
shaorui 0:c55310328157 722 }
shaorui 0:c55310328157 723 for (k=n-1; k>=j; k--){
shaorui 0:c55310328157 724 *(cpyC+i*n+k) -= *(cpyC+i*n+j) * *(cpyC+j*n+k);
shaorui 0:c55310328157 725 }
shaorui 0:c55310328157 726 }
shaorui 0:c55310328157 727 break;
shaorui 0:c55310328157 728 }
shaorui 0:c55310328157 729 }
shaorui 0:c55310328157 730
shaorui 0:c55310328157 731 if (l == n){ /* C[l][j] 全等于0*/
shaorui 0:c55310328157 732 free(cpyC);
shaorui 0:c55310328157 733 return NULL; /* 矩阵的行列式为零，不可求逆*/
shaorui 0:c55310328157 734 }
shaorui 0:c55310328157 735 }else{ /* C[n][n]等于0*/
shaorui 0:c55310328157 736 free(cpyC);
shaorui 0:c55310328157 737 return NULL; /* 矩阵的行列式为零，不可求逆*/
shaorui 0:c55310328157 738 }
shaorui 0:c55310328157 739 }
shaorui 0:c55310328157 740 /* 化成单位阵*/
shaorui 0:c55310328157 741 for(j=n-1; j>=1; j--){
shaorui 0:c55310328157 742 for(i=j-1; i>=0; i--){
shaorui 0:c55310328157 743 for(k=0; k<n; k++){
shaorui 0:c55310328157 744 *(IC+i*n+k) -= *(cpyC+i*n+j) * *(IC+j*n+k);
shaorui 0:c55310328157 745 }
shaorui 0:c55310328157 746 *(cpyC+i*n+j) = 0;
shaorui 0:c55310328157 747 }
shaorui 0:c55310328157 748 }
shaorui 0:c55310328157 749 free(cpyC);
shaorui 0:c55310328157 750 return IC;
shaorui 0:c55310328157 751 }
shaorui 0:c55310328157 752
shaorui 0:c55310328157 753
shaorui 0:c55310328157 754 /*============================ Mat3Inv =================================*/
shaorui 0:c55310328157 755 Mat3 kalmanfilter::Mat3Inv(cMat3 CM,Mat3 ICM){
shaorui 0:c55310328157 756 /*3*3矩阵的逆
shaorui 0:c55310328157 757 参数：[in]CM:3*3 CM矩阵
shaorui 0:c55310328157 758 [out]ICM:3*3 ICM矩阵
shaorui 0:c55310328157 759 返回：ICM,若不可逆则返回NULL
shaorui 0:c55310328157 760 */
shaorui 0:c55310328157 761
shaorui 0:c55310328157 762 /*需求：
shaorui 0:c55310328157 763 类型：Mat3,cMat3
shaorui 0:c55310328157 764 宏：NULL, EPS
shaorui 0:c55310328157 765 头文件: math.h
shaorui 0:c55310328157 766 */
shaorui 0:c55310328157 767
shaorui 0:c55310328157 768 /* 创建时间: 20070528
shaorui 0:c55310328157 769 最近更改时间：20070528
shaorui 0:c55310328157 770 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 771 */
shaorui 0:c55310328157 772
shaorui 0:c55310328157 773
shaorui 0:c55310328157 774 double det;
shaorui 0:c55310328157 775 double Ctemp[3][3];
shaorui 0:c55310328157 776 int i,j;
shaorui 0:c55310328157 777 det=CM[0][0]*CM[1][1]*CM[2][2]+CM[0][1]*CM[1][2]*CM[2][0]+CM[1][0]*CM[2][1]*CM[0][2]-CM[0][2]*CM[1][1]*CM[2][0]-CM[0][1]*CM[1][0]*CM[2][2]-CM[2][1]*CM[1][2]*CM[0][0];
shaorui 0:c55310328157 778
shaorui 0:c55310328157 779 if (fabs(det)<1.0e-16){
shaorui 0:c55310328157 780 return NULL;
shaorui 0:c55310328157 781 }
shaorui 0:c55310328157 782
shaorui 0:c55310328157 783 Ctemp[0][0] = (CM[1][1]*CM[2][2] - CM[2][1]*CM[1][2])/det;
shaorui 0:c55310328157 784 Ctemp[0][1] = -(CM[0][1]*CM[2][2] - CM[2][1]*CM[0][2])/det;
shaorui 0:c55310328157 785 Ctemp[0][2] = (CM[0][1]*CM[1][2] - CM[1][1]*CM[0][2])/det;
shaorui 0:c55310328157 786 Ctemp[1][0] = -(CM[1][0]*CM[2][2] - CM[2][0]*CM[1][2])/det;
shaorui 0:c55310328157 787 Ctemp[1][1] = (CM[0][0]*CM[2][2] - CM[2][0]*CM[0][2])/det;
shaorui 0:c55310328157 788 Ctemp[1][2] = -(CM[0][0]*CM[1][2] - CM[1][0]*CM[0][2])/det;
shaorui 0:c55310328157 789 Ctemp[2][0] = (CM[1][0]*CM[2][1] - CM[2][0]*CM[1][1])/det;
shaorui 0:c55310328157 790 Ctemp[2][1] = -(CM[0][0]*CM[2][1] - CM[2][0]*CM[0][1])/det;
shaorui 0:c55310328157 791 Ctemp[2][2] = (CM[0][0]*CM[1][1] - CM[0][1]*CM[1][0])/det;
shaorui 0:c55310328157 792 for (i=0;i<3;i++){
shaorui 0:c55310328157 793 for(j=0;j<3;j++){
shaorui 0:c55310328157 794 ICM[i][j]=Ctemp[i][j];
shaorui 0:c55310328157 795 }
shaorui 0:c55310328157 796 }
shaorui 0:c55310328157 797 return ICM;
shaorui 0:c55310328157 798 }
shaorui 0:c55310328157 799
shaorui 0:c55310328157 800
shaorui 0:c55310328157 801 /*============================ MatInvDiag =================================*/
shaorui 0:c55310328157 802 Mat kalmanfilter::MatInvDiag(cMat C,Mat IC,int n){
shaorui 0:c55310328157 803 /*方阵的对角逆
shaorui 0:c55310328157 804 参数：[in]C:方阵C
shaorui 0:c55310328157 805 [out]IC:方阵C的对角逆
shaorui 0:c55310328157 806 [in]n:方阵C的维数
shaorui 0:c55310328157 807 返回：IC,对角元素取倒数，其他元素置0,对角元素若为0则仍保持0
shaorui 0:c55310328157 808 */
shaorui 0:c55310328157 809
shaorui 0:c55310328157 810 /*需求：
shaorui 0:c55310328157 811 头文件:math.h,stdlib.h
shaorui 0:c55310328157 812 类型:Mat,cMat
shaorui 0:c55310328157 813 宏:MGet
shaorui 0:c55310328157 814 */
shaorui 0:c55310328157 815
shaorui 0:c55310328157 816 /* 创建时间: 20091104
shaorui 0:c55310328157 817 最近更改时间: 20091104
shaorui 0:c55310328157 818 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 819 */
shaorui 0:c55310328157 820
shaorui 0:c55310328157 821 int i,j;
shaorui 0:c55310328157 822 for (i=0;i<n;i++){
shaorui 0:c55310328157 823 for (j=0;j<n;j++){
shaorui 0:c55310328157 824 if(i!=j){
shaorui 0:c55310328157 825 MGet(IC,i,j,n)=0;
shaorui 0:c55310328157 826 }else{
shaorui 0:c55310328157 827 if (MGet(C,i,j,n)!=0){
shaorui 0:c55310328157 828 MGet(IC,i,j,n)=1.0/MGet(C,i,j,n);
shaorui 0:c55310328157 829 }else{
shaorui 0:c55310328157 830 MGet(IC,i,j,n)=0;
shaorui 0:c55310328157 831 }
shaorui 0:c55310328157 832 }
shaorui 0:c55310328157 833 }
shaorui 0:c55310328157 834 }
shaorui 0:c55310328157 835 return IC;
shaorui 0:c55310328157 836 }
shaorui 0:c55310328157 837
shaorui 0:c55310328157 838 /*****************************************************************************************************************************/
shaorui 0:c55310328157 839
shaorui 0:c55310328157 840 /*============================ MV3Prod =================================*/
shaorui 0:c55310328157 841 Vec3 kalmanfilter::MV3Prod(cMat3 C, cVec3 V, Vec3 Vout){
shaorui 0:c55310328157 842 /*3*3矩阵与3维向量乘法
shaorui 0:c55310328157 843 参数：[in]C:3*3 C矩阵
shaorui 0:c55310328157 844 [in]V:3维向量V
shaorui 0:c55310328157 845 [out]Vout: C*V(V视为列向量)
shaorui 0:c55310328157 846 返回：Vout
shaorui 0:c55310328157 847 */
shaorui 0:c55310328157 848
shaorui 0:c55310328157 849 /*需求：
shaorui 0:c55310328157 850 类型: Mat3,Vec3,cMat3,cVec3
shaorui 0:c55310328157 851 */
shaorui 0:c55310328157 852
shaorui 0:c55310328157 853 /* 创建时间: 20070528
shaorui 0:c55310328157 854 最近更改时间：20070528
shaorui 0:c55310328157 855 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 856 */
shaorui 0:c55310328157 857 double Vtemp[3];
shaorui 0:c55310328157 858 int i;
shaorui 0:c55310328157 859 for (i=0;i<3;i++){
shaorui 0:c55310328157 860 Vtemp[i] = C[i][0]*V[0] + C[i][1]*V[1] + C[i][2]*V[2];
shaorui 0:c55310328157 861 }
shaorui 0:c55310328157 862 Vout[0]=Vtemp[0];
shaorui 0:c55310328157 863 Vout[1]=Vtemp[1];
shaorui 0:c55310328157 864 Vout[2]=Vtemp[2];
shaorui 0:c55310328157 865 return Vout;
shaorui 0:c55310328157 866 }
shaorui 0:c55310328157 867
shaorui 0:c55310328157 868
shaorui 0:c55310328157 869 /*============================ VM3Prod =================================*/
shaorui 0:c55310328157 870 Vec3 kalmanfilter::VM3Prod(cVec3 V, cMat3 C, Vec3 Vout){
shaorui 0:c55310328157 871 /*3维向量与3*3矩阵乘法
shaorui 0:c55310328157 872 参数：[in]V:3维向量V
shaorui 0:c55310328157 873 [in]C:3*3 C矩阵
shaorui 0:c55310328157 874 [out]Vout: V*C(V视为行向量)
shaorui 0:c55310328157 875 返回：Vout
shaorui 0:c55310328157 876 */
shaorui 0:c55310328157 877
shaorui 0:c55310328157 878 /*需求：
shaorui 0:c55310328157 879 类型: Mat3,Vec3,cMat3,cVec3
shaorui 0:c55310328157 880 */
shaorui 0:c55310328157 881
shaorui 0:c55310328157 882 /* 创建时间: 20080402
shaorui 0:c55310328157 883 最近更改时间：20080402
shaorui 0:c55310328157 884 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 885 */
shaorui 0:c55310328157 886 double Vtemp[3];
shaorui 0:c55310328157 887 int i;
shaorui 0:c55310328157 888 for (i=0;i<3;i++){
shaorui 0:c55310328157 889 Vtemp[i] = C[0][i]*V[0] + C[1][i]*V[1] + C[2][i]*V[2];
shaorui 0:c55310328157 890 }
shaorui 0:c55310328157 891 Vout[0]=Vtemp[0];
shaorui 0:c55310328157 892 Vout[1]=Vtemp[1];
shaorui 0:c55310328157 893 Vout[2]=Vtemp[2];
shaorui 0:c55310328157 894 return Vout;
shaorui 0:c55310328157 895 }
shaorui 0:c55310328157 896
shaorui 0:c55310328157 897
shaorui 0:c55310328157 898 /*============================ MVProd =================================*/
shaorui 0:c55310328157 899 Vec kalmanfilter::MVProd(cMat C, cVec V, Vec Vout, int nM, int nN){
shaorui 0:c55310328157 900 /*矩阵与向量乘法
shaorui 0:c55310328157 901 参数：[in]C: C矩阵
shaorui 0:c55310328157 902 [in]V: 向量V
shaorui 0:c55310328157 903 [out]Vout: C*V(V视为列向量)
shaorui 0:c55310328157 904 [in]nM: C行数
shaorui 0:c55310328157 905 [in]nN: C列数,即V的维数
shaorui 0:c55310328157 906 返回：Vout
shaorui 0:c55310328157 907 */
shaorui 0:c55310328157 908
shaorui 0:c55310328157 909 /*需求：
shaorui 0:c55310328157 910 类型: Mat,Vec,cMat,cVec
shaorui 0:c55310328157 911 宏: MGet
shaorui 0:c55310328157 912 头文件: stdlib.h
shaorui 0:c55310328157 913 */
shaorui 0:c55310328157 914
shaorui 0:c55310328157 915 /* 创建时间: 20080402
shaorui 0:c55310328157 916 最近更改时间：20080402
shaorui 0:c55310328157 917 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 918 */
shaorui 0:c55310328157 919 Vec Vtemp=(Vec)malloc(sizeof(double)*nM);
shaorui 0:c55310328157 920 int i,j;
shaorui 0:c55310328157 921 for (i=0;i<nM;i++){
shaorui 0:c55310328157 922 Vtemp[i]=0.0;
shaorui 0:c55310328157 923 for(j=0;j<nN;j++){
shaorui 0:c55310328157 924 Vtemp[i] = Vtemp[i]+MGet(C,i,j,nN)*V[j];
shaorui 0:c55310328157 925 }
shaorui 0:c55310328157 926 }
shaorui 0:c55310328157 927 for (i=0;i<nM;i++){
shaorui 0:c55310328157 928 Vout[i]=Vtemp[i];
shaorui 0:c55310328157 929 }
shaorui 0:c55310328157 930 free(Vtemp);
shaorui 0:c55310328157 931 return Vout;
shaorui 0:c55310328157 932 }
shaorui 0:c55310328157 933
shaorui 0:c55310328157 934
shaorui 0:c55310328157 935 /*============================ VMProd =================================*/
shaorui 0:c55310328157 936 Vec kalmanfilter::VMProd(cVec V, cMat C, Vec Vout, int nM, int nN){
shaorui 0:c55310328157 937 /*向量与矩阵乘法
shaorui 0:c55310328157 938 参数：[in]V: 向量V
shaorui 0:c55310328157 939 [in]C: 矩阵C
shaorui 0:c55310328157 940 [out]Vout: V*C(V视为行向量)
shaorui 0:c55310328157 941 [in]nM: C行数,即V的维数
shaorui 0:c55310328157 942 [in]nN: C列数
shaorui 0:c55310328157 943 返回：Vout
shaorui 0:c55310328157 944 */
shaorui 0:c55310328157 945
shaorui 0:c55310328157 946 /*需求：
shaorui 0:c55310328157 947 类型: Mat,Vec,cMat,cVec
shaorui 0:c55310328157 948 宏: MGet
shaorui 0:c55310328157 949 头文件: stdlib.h
shaorui 0:c55310328157 950 */
shaorui 0:c55310328157 951
shaorui 0:c55310328157 952 /* 创建时间: 20080402
shaorui 0:c55310328157 953 最近更改时间：20080402
shaorui 0:c55310328157 954 Copyright 2005-2010 SSTC
shaorui 0:c55310328157 955 */
shaorui 0:c55310328157 956
shaorui 0:c55310328157 957 Vec Vtemp=(Vec)malloc(sizeof(double)*nN);
shaorui 0:c55310328157 958 int i,j;
shaorui 0:c55310328157 959 for (i=0;i<nN;i++){
shaorui 0:c55310328157 960 Vtemp[i]=0.0;
shaorui 0:c55310328157 961 for(j=0;j<nM;j++){
shaorui 0:c55310328157 962 Vtemp[i] = Vtemp[i]+MGet(C,j,i,nN)*V[j];
shaorui 0:c55310328157 963 }
shaorui 0:c55310328157 964 }
shaorui 0:c55310328157 965 for (i=0;i<nN;i++){
shaorui 0:c55310328157 966 Vout[i]=Vtemp[i];
shaorui 0:c55310328157 967 }
shaorui 0:c55310328157 968 free(Vtemp);
shaorui 0:c55310328157 969 return Vout;
shaorui 0:c55310328157 970 }
shaorui 0:c55310328157 971 /**************************************************************************************************************/
shaorui 0:c55310328157 972
shaorui 0:c55310328157 973 /*========================= VecCpy =================================*/
shaorui 0:c55310328157 974 Vec kalmanfilter::VecCpy(Vec dest, cVec source, int nOrder){
shaorui 0:c55310328157 975 /*向量拷贝
shaorui 0:c55310328157 976 参数：[out]dest: 目标向量
shaorui 0:c55310328157 977 [in]source: 源向量
shaorui 0:c55310328157 978 [in]nOrder: 向量维数
shaorui 0:c55310328157 979 返回：dest
shaorui 0:c55310328157 980 */
shaorui 0:c55310328157 981
shaorui 0:c55310328157 982 /*需求：
shaorui 0:c55310328157 983 类型: Vec,cVec
shaorui 0:c55310328157 984 */
shaorui 0:c55310328157 985
shaorui 0:c55310328157 986 /*创建时间: 20070530
shaorui 0:c55310328157 987 最近更改时间：20070530
shaorui 0:c55310328157 988 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 989 Email: nostalgica@163.com
shaorui 0:c55310328157 990 */
shaorui 0:c55310328157 991 int i;
shaorui 0:c55310328157 992 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 993 dest[i]=source[i];
shaorui 0:c55310328157 994 }
shaorui 0:c55310328157 995 return dest;
shaorui 0:c55310328157 996 }
shaorui 0:c55310328157 997
shaorui 0:c55310328157 998
shaorui 0:c55310328157 999 /*========================= nVecCpy =================================*/
shaorui 0:c55310328157 1000 nVec kalmanfilter::nVecCpy(nVec dest, cnVec source, int nOrder){
shaorui 0:c55310328157 1001 /*整型向量拷贝
shaorui 0:c55310328157 1002 参数：[out]dest: 目标向量
shaorui 0:c55310328157 1003 [in]source: 源向量
shaorui 0:c55310328157 1004 [in]nOrder: 向量维数
shaorui 0:c55310328157 1005 返回：dest
shaorui 0:c55310328157 1006 */
shaorui 0:c55310328157 1007
shaorui 0:c55310328157 1008 /*需求：
shaorui 0:c55310328157 1009 类型: nVec,cnVec
shaorui 0:c55310328157 1010 */
shaorui 0:c55310328157 1011
shaorui 0:c55310328157 1012 /*创建时间: 20080512
shaorui 0:c55310328157 1013 最近更改时间：20080512
shaorui 0:c55310328157 1014 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 1015 Email: nostalgica@163.com
shaorui 0:c55310328157 1016 */
shaorui 0:c55310328157 1017 int i;
shaorui 0:c55310328157 1018 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 1019 dest[i]=source[i];
shaorui 0:c55310328157 1020 }
shaorui 0:c55310328157 1021 return dest;
shaorui 0:c55310328157 1022 }
shaorui 0:c55310328157 1023 /*********************************************************************************************************/
shaorui 0:c55310328157 1024
shaorui 0:c55310328157 1025 /*========================= VecSum ====================================*/
shaorui 0:c55310328157 1026 Vec kalmanfilter::VecSum(cVec V1, cVec V2, int nOrder, Vec Vout){
shaorui 0:c55310328157 1027 /*向量加法
shaorui 0:c55310328157 1028 参数：[in]V1: 向量1
shaorui 0:c55310328157 1029 [in]V2: 向量2
shaorui 0:c55310328157 1030 [in]nOrder: 向量1与向量2的维数
shaorui 0:c55310328157 1031 [out]Vout: 向量1与向量2的和
shaorui 0:c55310328157 1032 返回：Vout
shaorui 0:c55310328157 1033 */
shaorui 0:c55310328157 1034
shaorui 0:c55310328157 1035 /*需求：
shaorui 0:c55310328157 1036 类型: Vec,cVec
shaorui 0:c55310328157 1037 */
shaorui 0:c55310328157 1038
shaorui 0:c55310328157 1039 /*创建时间: 20070530
shaorui 0:c55310328157 1040 最近更改时间：20070530
shaorui 0:c55310328157 1041 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 1042 Email: nostalgica@163.com
shaorui 0:c55310328157 1043 */
shaorui 0:c55310328157 1044 int i;
shaorui 0:c55310328157 1045 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 1046 Vout[i]=V1[i]+V2[i];
shaorui 0:c55310328157 1047 }
shaorui 0:c55310328157 1048 return Vout;
shaorui 0:c55310328157 1049 }
shaorui 0:c55310328157 1050
shaorui 0:c55310328157 1051
shaorui 0:c55310328157 1052 /*========================= nVecSum ====================================*/
shaorui 0:c55310328157 1053 nVec kalmanfilter::nVecSum(cnVec V1, cnVec V2, int nOrder, nVec Vout){
shaorui 0:c55310328157 1054 /*整型向量加法
shaorui 0:c55310328157 1055 参数：[in]V1: 整型向量1
shaorui 0:c55310328157 1056 [in]V2: 整型向量2
shaorui 0:c55310328157 1057 [in]nOrder: 向量1与向量2的维数
shaorui 0:c55310328157 1058 [out]Vout: 向量1与向量2的和
shaorui 0:c55310328157 1059 返回：Vout
shaorui 0:c55310328157 1060 */
shaorui 0:c55310328157 1061
shaorui 0:c55310328157 1062 /*需求：
shaorui 0:c55310328157 1063 类型: nVec,cnVec
shaorui 0:c55310328157 1064 */
shaorui 0:c55310328157 1065
shaorui 0:c55310328157 1066 /*创建时间: 20080512
shaorui 0:c55310328157 1067 最近更改时间：20080512
shaorui 0:c55310328157 1068 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 1069 Email: nostalgica@163.com
shaorui 0:c55310328157 1070 */
shaorui 0:c55310328157 1071 int i;
shaorui 0:c55310328157 1072 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 1073 Vout[i]=V1[i]+V2[i];
shaorui 0:c55310328157 1074 }
shaorui 0:c55310328157 1075 return Vout;
shaorui 0:c55310328157 1076 }
shaorui 0:c55310328157 1077 /*******************************************************************************************************************/
shaorui 0:c55310328157 1078
shaorui 0:c55310328157 1079 /*========================= VecMinus =================================*/
shaorui 0:c55310328157 1080 Vec kalmanfilter::VecMinus(cVec V1, cVec V2, int nOrder, Vec Vout){
shaorui 0:c55310328157 1081 /*向量减法
shaorui 0:c55310328157 1082 参数：[in]V1: 向量1
shaorui 0:c55310328157 1083 [in]V2: 向量2
shaorui 0:c55310328157 1084 [in]nOrder: 向量1与向量2的维数
shaorui 0:c55310328157 1085 [out]Vout: 向量1与向量2的差
shaorui 0:c55310328157 1086 返回：Vout
shaorui 0:c55310328157 1087 */
shaorui 0:c55310328157 1088
shaorui 0:c55310328157 1089 /*需求：
shaorui 0:c55310328157 1090 类型: Vec,cVec
shaorui 0:c55310328157 1091 */
shaorui 0:c55310328157 1092
shaorui 0:c55310328157 1093 /*创建时间: 20070530
shaorui 0:c55310328157 1094 最近更改时间：20070530
shaorui 0:c55310328157 1095 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 1096 Email: nostalgica@163.com
shaorui 0:c55310328157 1097 */
shaorui 0:c55310328157 1098 int i;
shaorui 0:c55310328157 1099 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 1100 Vout[i]=V1[i]-V2[i];
shaorui 0:c55310328157 1101 }
shaorui 0:c55310328157 1102 return Vout;
shaorui 0:c55310328157 1103 }
shaorui 0:c55310328157 1104
shaorui 0:c55310328157 1105
shaorui 0:c55310328157 1106 /*========================= nVecMinus =================================*/
shaorui 0:c55310328157 1107 nVec kalmanfilter::nVecMinus(cnVec V1, cnVec V2, int nOrder, nVec Vout){
shaorui 0:c55310328157 1108 /*整型向量减法
shaorui 0:c55310328157 1109 参数：[in]V1: 整型向量1
shaorui 0:c55310328157 1110 [in]V2: 整型向量2
shaorui 0:c55310328157 1111 [in]nOrder: 向量1与向量2的维数
shaorui 0:c55310328157 1112 [out]Vout: 向量1与向量2的差
shaorui 0:c55310328157 1113 返回：Vout
shaorui 0:c55310328157 1114 */
shaorui 0:c55310328157 1115
shaorui 0:c55310328157 1116 /*需求：
shaorui 0:c55310328157 1117 类型: nVec,cnVec
shaorui 0:c55310328157 1118 */
shaorui 0:c55310328157 1119
shaorui 0:c55310328157 1120 /*创建时间: 20080512
shaorui 0:c55310328157 1121 最近更改时间：20080512
shaorui 0:c55310328157 1122 Copyright 2006-2010 SSTC,HLW
shaorui 0:c55310328157 1123 Email: nostalgica@163.com
shaorui 0:c55310328157 1124 */
shaorui 0:c55310328157 1125 int i;
shaorui 0:c55310328157 1126 for (i=0;i<nOrder;i++){
shaorui 0:c55310328157 1127 Vout[i]=V1[i]-V2[i];
shaorui 0:c55310328157 1128 }
shaorui 0:c55310328157 1129 return Vout;
shaorui 0:c55310328157 1130 }
shaorui 0:c55310328157 1131
shaorui 0:c55310328157 1132 /*******************************************************************************************************************/
shaorui 0:c55310328157 1133
shaorui 0:c55310328157 1134 /*========================= Kalman1 ====================================*/
shaorui 0:c55310328157 1135 Vec kalmanfilter::Kalman1(cMat Phik_, cMat Gamak_1, cMat Hk, cMat Qk_1, cMat Rk, cMat Pk_1, cVec Xk_1, cVec Zk, cMat Bk_1, cVec Uk_1,
shaorui 0:c55310328157 1136 int n, int m, int q, int p, Vec Xk, Mat Pk, Mat Kk, Mat Pk_, Vec Xk_){
shaorui 0:c55310328157 1137 /*离散卡尔曼滤波的一步递推,系统方程：X(k+1)=Φ(k+1,k)X(k)+B(k)U(k)+Γ(k)W(k),Z(k+1)=H(k+1)X(k+1)+V(k+1)
shaorui 0:c55310328157 1138 参数：[in]Phik_: Φ(k+1,k);n*n,n为阶数
shaorui 0:c55310328157 1139 [in]Gamak_1: Γ(k);n*q,q为驱动噪声W(k)维数
shaorui 0:c55310328157 1140 [in]Hk: H(k+1);m*n,m为观测维数
shaorui 0:c55310328157 1141 [in]Qk_1: Q(k);q*q,驱动噪声W(k)协方差阵
shaorui 0:c55310328157 1142 [in]Rk: R(k+1);m*m,观测噪声V(k+1)协方差阵
shaorui 0:c55310328157 1143 [in]Pk_1: P(k,k);n*n
shaorui 0:c55310328157 1144 [in]Xk_1: X(k,k);n维向量
shaorui 0:c55310328157 1145 [in]Zk: Z(k+1);m维向量
shaorui 0:c55310328157 1146 [in]Bk_1: B(k);n*p,p为输入维数
shaorui 0:c55310328157 1147 [in]Uk_1: U(k);p维向量
shaorui 0:c55310328157 1148 [in]n: 系统阶数
shaorui 0:c55310328157 1149 [in]m: 观测维数
shaorui 0:c55310328157 1150 [in]p: 输入维数
shaorui 0:c55310328157 1151 [in]q: 驱动噪声维数
shaorui 0:c55310328157 1152 [out]Xk: X(k+1,k+1);n维向量,若不需输出设为NULL
shaorui 0:c55310328157 1153 [out]Pk: P(k+1,k+1);n*n,若不需输出设为NULL
shaorui 0:c55310328157 1154 [out]Kk: K(k+1);n*m,若不需输出设为NULL
shaorui 0:c55310328157 1155 [out]Pk_: P(k+1,k);n*n,若不需输出设为NULL
shaorui 0:c55310328157 1156 [out]Xk_: X(k+1,k);n维向量,若不需输出设为NULL
shaorui 0:c55310328157 1157 返回：Xk,若不成功,返回NULL
shaorui 0:c55310328157 1158 */
shaorui 0:c55310328157 1159
shaorui 0:c55310328157 1160 /*需求：
shaorui 0:c55310328157 1161 函数: MatTrans, MatProd, MatSum, MatInv, MatInitI, MatMinus, MVProd, VecSum, VecMinus, VecCpy, MatCpy
shaorui 0:c55310328157 1162 头文件: stdlib.h
shaorui 0:c55310328157 1163 宏: NULL
shaorui 0:c55310328157 1164 */
shaorui 0:c55310328157 1165
shaorui 0:c55310328157 1166 /* 创建时间: 20080402
shaorui 0:c55310328157 1167 最近更改时间：20080403
shaorui 0:c55310328157 1168 Copyright 2005-2010 SSTC,HLW
shaorui 0:c55310328157 1169 Email: nostalgica@163.com
shaorui 0:c55310328157 1170 */
shaorui 0:c55310328157 1171
shaorui 0:c55310328157 1172 int nmax=max(max(max(n,m),p),q);
shaorui 0:c55310328157 1173 Mat tCn=(Mat)malloc(sizeof(double)*nmax*nmax);
shaorui 0:c55310328157 1174 Mat tCn2=(Mat)malloc(sizeof(double)*nmax*nmax);
shaorui 0:c55310328157 1175 Mat tCn3=(Mat)malloc(sizeof(double)*nmax*nmax);
shaorui 0:c55310328157 1176 Mat tCn4=(Mat)malloc(sizeof(double)*nmax*nmax);
shaorui 0:c55310328157 1177 Mat tCn5=(Mat)malloc(sizeof(double)*nmax*nmax);
shaorui 0:c55310328157 1178 Vec tV=(Vec)malloc(sizeof(double)*nmax);
shaorui 0:c55310328157 1179 Vec tV2=(Vec)malloc(sizeof(double)*nmax);
shaorui 0:c55310328157 1180
shaorui 0:c55310328157 1181 /*Pk_=Phik_*Pk_1*Phik_'+Gamak_1*Qk_1*Gamak_1';
shaorui 0:c55310328157 1182 Kk=Pk_*Hk'*(Hk*Pk_*Hk'+Rk)^(-1);
shaorui 0:c55310328157 1183 I=eye(order);
shaorui 0:c55310328157 1184 Pk=(I-Kk*Hk)*Pk_*(I-Kk*Hk)'+Kk*Rk*Kk';
shaorui 0:c55310328157 1185 Xk_=Phik_*Xk_1+Bk_1*Uk_1;
shaorui 0:c55310328157 1186 Xk=Xk_+Kk*(Zk-Hk*Xk_);
shaorui 0:c55310328157 1187 */
shaorui 0:c55310328157 1188
shaorui 0:c55310328157 1189 MatTrans(Phik_,tCn,n,n);
shaorui 0:c55310328157 1190 MatProd(Pk_1,tCn,tCn2,n,n,n);
shaorui 0:c55310328157 1191 MatProd(Phik_,tCn2,tCn,n,n,n);
shaorui 0:c55310328157 1192 MatTrans(Gamak_1,tCn2,n,q);
shaorui 0:c55310328157 1193 MatProd(Qk_1,tCn2,tCn3,q,q,n);
shaorui 0:c55310328157 1194 MatProd(Gamak_1,tCn3,tCn2,n,q,n);
shaorui 0:c55310328157 1195 MatSum(tCn,tCn2,tCn,n,n);/*tCn:Pk_*/
shaorui 0:c55310328157 1196
shaorui 0:c55310328157 1197 MatTrans(Hk,tCn2,m,n);
shaorui 0:c55310328157 1198 MatProd(Hk,MatProd(tCn,tCn2,tCn3,n,n,m),tCn4,m,n,m);
shaorui 0:c55310328157 1199 MatSum(tCn4,Rk,tCn3,m,m);
shaorui 0:c55310328157 1200 if (MatInv(tCn3,tCn3,m)==NULL){
shaorui 0:c55310328157 1201 return NULL;
shaorui 0:c55310328157 1202 }
shaorui 0:c55310328157 1203 MatProd(tCn,MatProd(tCn2,tCn3,tCn4,n,m,m),tCn2,n,n,m);/*tCn2:Kk*/
shaorui 0:c55310328157 1204 MatInitI(tCn5,n);
shaorui 0:c55310328157 1205
shaorui 0:c55310328157 1206 MatMinus(tCn5,MatProd(tCn2,Hk,tCn3,n,m,n),tCn3,n,n);
shaorui 0:c55310328157 1207 MatTrans(tCn3,tCn4,n,n);
shaorui 0:c55310328157 1208 MatProd(tCn3,MatProd(tCn,tCn4,tCn4,n,n,n),tCn3,n,n,n);
shaorui 0:c55310328157 1209
shaorui 0:c55310328157 1210 MatProd(tCn2,MatProd(Rk,MatTrans(tCn2,tCn4,n,m),tCn4,m,m,n),tCn4,n,m,n);
shaorui 0:c55310328157 1211 MatSum(tCn3,tCn4,tCn3,n,n);/*tCn3:Pk*/
shaorui 0:c55310328157 1212 MVProd(Phik_,Xk_1,tV,n,n);
shaorui 0:c55310328157 1213 MVProd(Bk_1,Uk_1,tV2,n,p);
shaorui 0:c55310328157 1214 VecSum(tV,tV2,n,tV);/*tV:Xk_*/
shaorui 0:c55310328157 1215 VecMinus(Zk,MVProd(Hk,tV,tV2,m,n),m,tV2);
shaorui 0:c55310328157 1216 VecSum(tV,MVProd(tCn2,tV2,tV2,n,m),n,tV2);/*tV2:Xk*/
shaorui 0:c55310328157 1217
shaorui 0:c55310328157 1218
shaorui 0:c55310328157 1219 if (Xk!=NULL){
shaorui 0:c55310328157 1220 VecCpy(Xk,tV2,n);
shaorui 0:c55310328157 1221 }
shaorui 0:c55310328157 1222 if (Pk!=NULL){
shaorui 0:c55310328157 1223 MatCpy(Pk,tCn3,n,n);
shaorui 0:c55310328157 1224 }
shaorui 0:c55310328157 1225 if (Kk!=NULL){
shaorui 0:c55310328157 1226 MatCpy(Kk,tCn2,n,m);
shaorui 0:c55310328157 1227 }
shaorui 0:c55310328157 1228 if (Pk_!=NULL){
shaorui 0:c55310328157 1229 MatCpy(Pk_,tCn,n,n);
shaorui 0:c55310328157 1230 }
shaorui 0:c55310328157 1231 if (Xk_!=NULL){
shaorui 0:c55310328157 1232 VecCpy(Xk_,tV,n);
shaorui 0:c55310328157 1233 }
shaorui 0:c55310328157 1234
shaorui 0:c55310328157 1235 free(tCn);
shaorui 0:c55310328157 1236 free(tCn2);
shaorui 0:c55310328157 1237 free(tCn3);
shaorui 0:c55310328157 1238 free(tCn4);
shaorui 0:c55310328157 1239 free(tCn5);
shaorui 0:c55310328157 1240 free(tV);
shaorui 0:c55310328157 1241 free(tV2);
shaorui 0:c55310328157 1242 return Xk;
shaorui 0:c55310328157 1243 }