// Write the method information portion of ClassFile structure
// JVMSpec| u2 methods_count;
// JVMSpec| method_info methods[methods_count];
void JvmtiClassFileReconstituter::write_method_infos() {
HandleMark hm(thread());
objArrayHandle methods(thread(), ikh()->methods());
int num_methods = methods->length();
write_u2(num_methods);
if (JvmtiExport::can_maintain_original_method_order()) {
int index;
int original_index;
int* method_order = NEW_RESOURCE_ARRAY(int, num_methods);
// invert the method order mapping
for (index = 0; index < num_methods; index++) {
original_index = ikh()->method_ordering()->int_at(index);
assert(original_index >= 0 && original_index < num_methods,
"invalid original method index");
method_order[original_index] = index;
}
// write in original order
for (original_index = 0; original_index < num_methods; original_index++) {
index = method_order[original_index];
methodHandle method(thread(), (methodOop)(ikh()->methods()->obj_at(index)));
write_method_info(method);
}
} else {
// method order not preserved just dump the method infos
for (int index = 0; index < num_methods; index++) {
// Write SourceFile attribute
// JVMSpec| SourceFile_attribute {
// JVMSpec| u2 attribute_name_index;
// JVMSpec| u4 attribute_length;
// JVMSpec| u2 sourcefile_index;
// JVMSpec| }
void JvmtiClassFileReconstituter::write_source_file_attribute() {
assert(ikh()->source_file_name() != NULL, "caller must check");
write_attribute_name_index("SourceFile");
write_u4(2); // always length 2
write_u2(symbol_to_cpool_index(ikh()->source_file_name()));
}
// Write SourceDebugExtension attribute
// JSR45| SourceDebugExtension_attribute {
// JSR45| u2 attribute_name_index;
// JSR45| u4 attribute_length;
// JSR45| u1 debug_extension[attribute_length];
// JSR45| }
void JvmtiClassFileReconstituter::write_source_debug_extension_attribute() {
assert(ikh()->source_debug_extension() != NULL, "caller must check");
write_attribute_name_index("SourceDebugExtension");
int len = (int)strlen(ikh()->source_debug_extension());
write_u4(len);
u1* ext = (u1*)ikh()->source_debug_extension();
for (int i=0; i<len; i++) {
write_u1(ext[i]);
}
}
// Write the method information portion of ClassFile structure
// JVMSpec| u2 methods_count;
// JVMSpec| method_info methods[methods_count];
void JvmtiClassFileReconstituter::write_method_infos() {
HandleMark hm(thread());
Array<Method*>* methods = ikh()->methods();
int num_methods = methods->length();
int num_overpass = 0;
// count the generated default interface methods
// these will not be re-created by write_method_info
// and should not be included in the total count
for (int index = 0; index < num_methods; index++) {
Method* method = methods->at(index);
if (method->is_overpass()) {
num_overpass++;
}
}
write_u2(num_methods - num_overpass);
if (JvmtiExport::can_maintain_original_method_order()) {
int index;
int original_index;
intArray method_order(num_methods, 0);
// invert the method order mapping
for (index = 0; index < num_methods; index++) {
original_index = ikh()->method_ordering()->at(index);
assert(original_index >= 0 && original_index < num_methods,
"invalid original method index");
method_order.at_put(original_index, index);
}
// write in original order
for (original_index = 0; original_index < num_methods; original_index++) {
index = method_order.at(original_index);
methodHandle method(thread(), methods->at(index));
write_method_info(method);
}
} else {
// method order not preserved just dump the method infos
for (int index = 0; index < num_methods; index++) {
methodHandle method(thread(), methods->at(index));
write_method_info(method);
}
}
}
void JvmtiBreakpoint::each_method_version_do(method_action meth_act) {
((methodOopDesc*)_method->*meth_act)(_bci);
// add/remove breakpoint to/from versions of the method that
// are EMCP. Directly or transitively obsolete methods are
// not saved in the PreviousVersionInfo.
Thread *thread = Thread::current();
instanceKlassHandle ikh = instanceKlassHandle(thread, _method->method_holder());
Symbol* m_name = _method->name();
Symbol* m_signature = _method->signature();
{
ResourceMark rm(thread);
// PreviousVersionInfo objects returned via PreviousVersionWalker
// contain a GrowableArray of handles. We have to clean up the
// GrowableArray _after_ the PreviousVersionWalker destructor
// has destroyed the handles.
{
// search previous versions if they exist
PreviousVersionWalker pvw((instanceKlass *)ikh()->klass_part());
for (PreviousVersionInfo * pv_info = pvw.next_previous_version();
pv_info != NULL; pv_info = pvw.next_previous_version()) {
GrowableArray<methodHandle>* methods =
pv_info->prev_EMCP_method_handles();
if (methods == NULL) {
// We have run into a PreviousVersion generation where
// all methods were made obsolete during that generation's
// RedefineClasses() operation. At the time of that
// operation, all EMCP methods were flushed so we don't
// have to go back any further.
//
// A NULL methods array is different than an empty methods
// array. We cannot infer any optimizations about older
// generations from an empty methods array for the current
// generation.
break;
}
for (int i = methods->length() - 1; i >= 0; i--) {
methodHandle method = methods->at(i);
if (method->name() == m_name && method->signature() == m_signature) {
RC_TRACE(0x00000800, ("%sing breakpoint in %s(%s)",
meth_act == &methodOopDesc::set_breakpoint ? "sett" : "clear",
method->name()->as_C_string(),
method->signature()->as_C_string()));
assert(!method->is_obsolete(), "only EMCP methods here");
((methodOopDesc*)method()->*meth_act)(_bci);
break;
}
}
}
} // pvw is cleaned up
} // rm is cleaned up
}
// Write the class attributes portion of ClassFile structure
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
void JvmtiClassFileReconstituter::write_class_attributes() {
u2 inner_classes_length = inner_classes_attribute_length();
Symbol* generic_signature = ikh()->generic_signature();
AnnotationArray* anno = ikh()->class_annotations();
AnnotationArray* type_anno = ikh()->class_type_annotations();
int attr_count = 0;
if (generic_signature != NULL) {
++attr_count;
}
if (ikh()->source_file_name() != NULL) {
++attr_count;
}
if (ikh()->source_debug_extension() != NULL) {
++attr_count;
}
if (inner_classes_length > 0) {
++attr_count;
}
if (anno != NULL) {
++attr_count; // has RuntimeVisibleAnnotations attribute
}
if (type_anno != NULL) {
++attr_count; // has RuntimeVisibleTypeAnnotations attribute
}
if (cpool()->operands() != NULL) {
++attr_count;
}
write_u2(attr_count);
if (generic_signature != NULL) {
write_signature_attribute(symbol_to_cpool_index(generic_signature));
}
if (ikh()->source_file_name() != NULL) {
write_source_file_attribute();
}
if (ikh()->source_debug_extension() != NULL) {
write_source_debug_extension_attribute();
}
if (inner_classes_length > 0) {
write_inner_classes_attribute(inner_classes_length);
}
if (anno != NULL) {
write_annotations_attribute("RuntimeVisibleAnnotations", anno);
}
if (type_anno != NULL) {
write_annotations_attribute("RuntimeVisibleTypeAnnotations", type_anno);
}
if (cpool()->operands() != NULL) {
write_bootstrapmethod_attribute();
}
}
void JvmtiBreakpoint::each_method_version_do(method_action meth_act) {
((Method*)_method->*meth_act)(_bci);
// add/remove breakpoint to/from versions of the method that
// are EMCP. Directly or transitively obsolete methods are
// not saved in the PreviousVersionNodes.
Thread *thread = Thread::current();
instanceKlassHandle ikh = instanceKlassHandle(thread, _method->method_holder());
Symbol* m_name = _method->name();
Symbol* m_signature = _method->signature();
// search previous versions if they exist
PreviousVersionWalker pvw(thread, (InstanceKlass *)ikh());
for (PreviousVersionNode * pv_node = pvw.next_previous_version();
pv_node != NULL; pv_node = pvw.next_previous_version()) {
GrowableArray<Method*>* methods = pv_node->prev_EMCP_methods();
if (methods == NULL) {
// We have run into a PreviousVersion generation where
// all methods were made obsolete during that generation's
// RedefineClasses() operation. At the time of that
// operation, all EMCP methods were flushed so we don't
// have to go back any further.
//
// A NULL methods array is different than an empty methods
// array. We cannot infer any optimizations about older
// generations from an empty methods array for the current
// generation.
break;
}
for (int i = methods->length() - 1; i >= 0; i--) {
Method* method = methods->at(i);
// obsolete methods that are running are not deleted from
// previous version array, but they are skipped here.
if (!method->is_obsolete() &&
method->name() == m_name &&
method->signature() == m_signature) {
RC_TRACE(0x00000800, ("%sing breakpoint in %s(%s)",
meth_act == &Method::set_breakpoint ? "sett" : "clear",
method->name()->as_C_string(),
method->signature()->as_C_string()));
(method->*meth_act)(_bci);
break;
}
}
}
}
// Write InnerClasses attribute
// JVMSpec| InnerClasses_attribute {
// JVMSpec| u2 attribute_name_index;
// JVMSpec| u4 attribute_length;
// JVMSpec| u2 number_of_classes;
// JVMSpec| { u2 inner_class_info_index;
// JVMSpec| u2 outer_class_info_index;
// JVMSpec| u2 inner_name_index;
// JVMSpec| u2 inner_class_access_flags;
// JVMSpec| } classes[number_of_classes];
// JVMSpec| }
void JvmtiClassFileReconstituter::write_inner_classes_attribute(int length) {
InnerClassesIterator iter(ikh());
guarantee(iter.length() != 0 && iter.length() == length,
"caller must check");
u2 entry_count = length / InstanceKlass::inner_class_next_offset;
u4 size = 2 + entry_count * (2+2+2+2);
write_attribute_name_index("InnerClasses");
write_u4(size);
write_u2(entry_count);
for (; !iter.done(); iter.next()) {
write_u2(iter.inner_class_info_index());
write_u2(iter.outer_class_info_index());
write_u2(iter.inner_name_index());
write_u2(iter.inner_access_flags());
}
}
void JvmtiClassFileReconstituter::write_class_file_format() {
ReallocMark();
// JVMSpec| ClassFile {
// JVMSpec| u4 magic;
write_u4(0xCAFEBABE);
// JVMSpec| u2 minor_version;
// JVMSpec| u2 major_version;
write_u2(ikh()->minor_version());
u2 major = ikh()->major_version();
write_u2(major);
// JVMSpec| u2 constant_pool_count;
// JVMSpec| cp_info constant_pool[constant_pool_count-1];
write_u2(cpool()->length());
copy_cpool_bytes(writeable_address(cpool_size()));
// JVMSpec| u2 access_flags;
write_u2(ikh()->access_flags().get_flags() & JVM_RECOGNIZED_CLASS_MODIFIERS);
// JVMSpec| u2 this_class;
// JVMSpec| u2 super_class;
write_u2(class_symbol_to_cpool_index(ikh()->name()));
Klass* super_class = ikh()->super();
write_u2(super_class == NULL? 0 : // zero for java.lang.Object
class_symbol_to_cpool_index(super_class->name()));
// JVMSpec| u2 interfaces_count;
// JVMSpec| u2 interfaces[interfaces_count];
Array<Klass*>* interfaces = ikh()->local_interfaces();
int num_interfaces = interfaces->length();
write_u2(num_interfaces);
for (int index = 0; index < num_interfaces; index++) {
HandleMark hm(thread());
instanceKlassHandle iikh(thread(), interfaces->at(index));
write_u2(class_symbol_to_cpool_index(iikh->name()));
}
// JVMSpec| u2 fields_count;
// JVMSpec| field_info fields[fields_count];
write_field_infos();
// JVMSpec| u2 methods_count;
// JVMSpec| method_info methods[methods_count];
write_method_infos();
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
// JVMSpec| } /* end ClassFile 8?
write_class_attributes();
}
// Write InnerClasses attribute
// JVMSpec| InnerClasses_attribute {
// JVMSpec| u2 attribute_name_index;
// JVMSpec| u4 attribute_length;
// JVMSpec| u2 number_of_classes;
// JVMSpec| { u2 inner_class_info_index;
// JVMSpec| u2 outer_class_info_index;
// JVMSpec| u2 inner_name_index;
// JVMSpec| u2 inner_class_access_flags;
// JVMSpec| } classes[number_of_classes];
// JVMSpec| }
void JvmtiClassFileReconstituter::write_inner_classes_attribute(int length) {
typeArrayOop inner_class_list = ikh()->inner_classes();
guarantee(inner_class_list != NULL && inner_class_list->length() == length,
"caller must check");
typeArrayHandle inner_class_list_h(thread(), inner_class_list);
assert (length % instanceKlass::inner_class_next_offset == 0, "just checking");
u2 entry_count = length / instanceKlass::inner_class_next_offset;
u4 size = 2 + entry_count * (2+2+2+2);
write_attribute_name_index("InnerClasses");
write_u4(size);
write_u2(entry_count);
for (int i = 0; i < length; i += instanceKlass::inner_class_next_offset) {
write_u2(inner_class_list_h->ushort_at(
i + instanceKlass::inner_class_inner_class_info_offset));
write_u2(inner_class_list_h->ushort_at(
i + instanceKlass::inner_class_outer_class_info_offset));
write_u2(inner_class_list_h->ushort_at(
i + instanceKlass::inner_class_inner_name_offset));
write_u2(inner_class_list_h->ushort_at(
i + instanceKlass::inner_class_access_flags_offset));
}
}
// Write the class attributes portion of ClassFile structure
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
void JvmtiClassFileReconstituter::write_class_attributes() {
u2 inner_classes_length = inner_classes_attribute_length();
Symbol* generic_signature = ikh()->generic_signature();
typeArrayHandle anno(thread(), ikh()->class_annotations());
int attr_count = 0;
if (generic_signature != NULL) {
++attr_count;
}
if (ikh()->source_file_name() != NULL) {
++attr_count;
}
if (ikh()->source_debug_extension() != NULL) {
++attr_count;
}
if (inner_classes_length > 0) {
++attr_count;
}
if (anno.not_null()) {
++attr_count; // has RuntimeVisibleAnnotations attribute
}
write_u2(attr_count);
if (generic_signature != NULL) {
write_signature_attribute(symbol_to_cpool_index(generic_signature));
}
if (ikh()->source_file_name() != NULL) {
write_source_file_attribute();
}
if (ikh()->source_debug_extension() != NULL) {
write_source_debug_extension_attribute();
}
if (inner_classes_length > 0) {
write_inner_classes_attribute(inner_classes_length);
}
if (anno.not_null()) {
write_annotations_attribute("RuntimeVisibleAnnotations", anno);
}
}
// Compute the number of entries in the InnerClasses attribute
u2 JvmtiClassFileReconstituter::inner_classes_attribute_length() {
typeArrayOop inner_class_list = ikh()->inner_classes();
return (inner_class_list == NULL) ? 0 : inner_class_list->length();
}
// Write the field information portion of ClassFile structure
// JVMSpec| u2 fields_count;
// JVMSpec| field_info fields[fields_count];
void JvmtiClassFileReconstituter::write_field_infos() {
HandleMark hm(thread());
typeArrayHandle fields(thread(), ikh()->fields());
int fields_length = fields->length();
int num_fields = fields_length / instanceKlass::next_offset;
objArrayHandle fields_anno(thread(), ikh()->fields_annotations());
write_u2(num_fields);
for (int index = 0; index < fields_length; index += instanceKlass::next_offset) {
AccessFlags access_flags;
int flags = fields->ushort_at(index + instanceKlass::access_flags_offset);
access_flags.set_flags(flags);
int name_index = fields->ushort_at(index + instanceKlass::name_index_offset);
int signature_index = fields->ushort_at(index + instanceKlass::signature_index_offset);
int initial_value_index = fields->ushort_at(index + instanceKlass::initval_index_offset);
guarantee(name_index != 0 && signature_index != 0, "bad constant pool index for field");
int offset = ikh()->offset_from_fields( index );
int generic_signature_index =
fields->ushort_at(index + instanceKlass::generic_signature_offset);
typeArrayHandle anno(thread(), fields_anno.not_null() ?
(typeArrayOop)(fields_anno->obj_at(index / instanceKlass::next_offset)) :
(typeArrayOop)NULL);
// JVMSpec| field_info {
// JVMSpec| u2 access_flags;
// JVMSpec| u2 name_index;
// JVMSpec| u2 descriptor_index;
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
// JVMSpec| }
write_u2(flags & JVM_RECOGNIZED_FIELD_MODIFIERS);
write_u2(name_index);
write_u2(signature_index);
int attr_count = 0;
if (initial_value_index != 0) {
++attr_count;
}
if (access_flags.is_synthetic()) {
// ++attr_count;
}
if (generic_signature_index != 0) {
++attr_count;
}
if (anno.not_null()) {
++attr_count; // has RuntimeVisibleAnnotations attribute
}
write_u2(attr_count);
if (initial_value_index != 0) {
write_attribute_name_index("ConstantValue");
write_u4(2); //length always 2
write_u2(initial_value_index);
}
if (access_flags.is_synthetic()) {
// write_synthetic_attribute();
}
if (generic_signature_index != 0) {
write_signature_attribute(generic_signature_index);
}
if (anno.not_null()) {
write_annotations_attribute("RuntimeVisibleAnnotations", anno);
}
}
}
original_index = ikh()->method_ordering()->int_at(index);
assert(original_index >= 0 && original_index < num_methods,
"invalid original method index");
method_order[original_index] = index;
}
// write in original order
for (original_index = 0; original_index < num_methods; original_index++) {
index = method_order[original_index];
methodHandle method(thread(), (methodOop)(ikh()->methods()->obj_at(index)));
write_method_info(method);
}
} else {
// method order not preserved just dump the method infos
for (int index = 0; index < num_methods; index++) {
methodHandle method(thread(), (methodOop)(ikh()->methods()->obj_at(index)));
write_method_info(method);
}
}
}
void JvmtiClassFileReconstituter::write_class_file_format() {
ReallocMark();
// JVMSpec| ClassFile {
// JVMSpec| u4 magic;
write_u4(0xCAFEBABE);
// JVMSpec| u2 minor_version;
// JVMSpec| u2 major_version;
write_u2(ikh()->minor_version());
// Write the field information portion of ClassFile structure
// JVMSpec| u2 fields_count;
// JVMSpec| field_info fields[fields_count];
void JvmtiClassFileReconstituter::write_field_infos() {
HandleMark hm(thread());
Array<AnnotationArray*>* fields_anno = ikh()->fields_annotations();
Array<AnnotationArray*>* fields_type_anno = ikh()->fields_type_annotations();
// Compute the real number of Java fields
int java_fields = ikh()->java_fields_count();
write_u2(java_fields);
for (JavaFieldStream fs(ikh()); !fs.done(); fs.next()) {
AccessFlags access_flags = fs.access_flags();
int name_index = fs.name_index();
int signature_index = fs.signature_index();
int initial_value_index = fs.initval_index();
guarantee(name_index != 0 && signature_index != 0, "bad constant pool index for field");
// int offset = ikh()->field_offset( index );
int generic_signature_index = fs.generic_signature_index();
AnnotationArray* anno = fields_anno == NULL ? NULL : fields_anno->at(fs.index());
AnnotationArray* type_anno = fields_type_anno == NULL ? NULL : fields_type_anno->at(fs.index());
// JVMSpec| field_info {
// JVMSpec| u2 access_flags;
// JVMSpec| u2 name_index;
// JVMSpec| u2 descriptor_index;
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
// JVMSpec| }
write_u2(access_flags.as_int() & JVM_RECOGNIZED_FIELD_MODIFIERS);
write_u2(name_index);
write_u2(signature_index);
int attr_count = 0;
if (initial_value_index != 0) {
++attr_count;
}
if (access_flags.is_synthetic()) {
// ++attr_count;
}
if (generic_signature_index != 0) {
++attr_count;
}
if (anno != NULL) {
++attr_count; // has RuntimeVisibleAnnotations attribute
}
if (type_anno != NULL) {
++attr_count; // has RuntimeVisibleTypeAnnotations attribute
}
write_u2(attr_count);
if (initial_value_index != 0) {
write_attribute_name_index("ConstantValue");
write_u4(2); //length always 2
write_u2(initial_value_index);
}
if (access_flags.is_synthetic()) {
// write_synthetic_attribute();
}
if (generic_signature_index != 0) {
write_signature_attribute(generic_signature_index);
}
if (anno != NULL) {
write_annotations_attribute("RuntimeVisibleAnnotations", anno);
}
if (type_anno != NULL) {
write_annotations_attribute("RuntimeVisibleTypeAnnotations", type_anno);
}
}
}
// Compute the number of entries in the InnerClasses attribute
u2 JvmtiClassFileReconstituter::inner_classes_attribute_length() {
InnerClassesIterator iter(ikh());
return iter.length();
}
//------------------------------------------------------------------------------
void ExtendedKalmanInv::UpdateCovarianceJoseph()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Updating covariance using Joseph "
"method\n");
#endif
// Update the covariance
Real khSum;
Rmatrix temp(stateSize, stateSize), krk(stateSize, stateSize);
Rmatrix pikh(stateSize, stateSize), r(measSize, measSize);
Rmatrix ikh(stateSize, stateSize), rk(measSize, stateSize);
// First calc (I - K H)
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage("Calcing I - K H\n K:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", kalman(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n H:\n");
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", hTilde[i][j]);
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
if (i == j)
ikh(i,j) = 1.0;
else
ikh(i,j) = 0.0;
khSum = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
khSum += kalman(i,k) * hTilde[k][j];
ikh(i,j) -= khSum;
}
}
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage(" I - K H:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", ikh(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n pBar:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", pBar(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n (I - K H) \bar P (I - K H)^T:\n");
#endif
// Build (I - K H) \bar P (I - K H)^T
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
pikh(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
pikh(i,j) += pBar(i,k) * ikh(j,k);
}
}
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
temp(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
temp(i,j) += ikh(i,k) * pikh(k,j);
}
}
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", temp(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage("Prepping to fake up R\n R:\n");
#endif
// Fake up the measurement covariance if needed
if (measCovariance)
r = *(measCovariance->GetCovariance());
else
for (UnsignedInt i = 0; i < measSize; ++i)
for (UnsignedInt j = 0; j < measSize; ++j)
if (i == j)
r(i,j) = DEFAULT_MEASUREMENT_COVARIANCE;
else
r(i,j) = 0.0;
// Now K R K^T
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", r(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Calcing RK:\n");
#endif
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
rk(i,j) = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
rk(i,j) += r(i,k) * kalman(j,k);
}
}
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", rk(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Calcing KRK:\n");
#endif
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
krk(i,j) = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
krk(i,j) += kalman(i,k) * rk(k,j);
}
}
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", krk(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Add 'em up
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage("Summing covariance\n");
#endif
for (UnsignedInt i = 0; i < stateSize; ++i)
for (UnsignedInt j = 0; j < stateSize; ++j)
(*stateCovariance)(i,j) = temp(i,j) + krk(i,j);
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", (*covariance)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
throw EstimatorException("Intentional debug break!");
#endif
}
